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ABSTRACT 


A  near-optimal  guidance  law  has  been  developed  using  the  direct  method  of 
calculus  of  variations  that  maximizes  the  kinetic  energy  transfer  from  a  surface-launched 
missile  upon  interception  to  a  ballistic  missile  target  during  the  boost  phase  of  flight. 
Mathematical  models  of  a  North  Korean  Taep'o-dong  II  (TD-2)  medium-range  ballistic 
missile  and  a  Raytheon  Standard  Missile  6  (SM-6)  interceptor  are  used  to  demonstrate 
the  guidance  law’s  perfonnance.  This  law  will  utilize  the  SM-6’s  onboard  computer  and 
active  radar  sensors  to  independently  predict  an  intercept  point,  solve  the  two-point 
boundary  value  problem,  and  determine  a  near-optimal  flight  path  to  that  point. 
Determining  a  truly  optimal  flight  path  would  require  significant  computing  power  and 
time,  while  a  near-optimal  flight  path  can  be  calculated  onboard  the  interceptor  and 
updated  in  real  time  without  significant  changes  to  the  interceptor’s  hardware.  That  near- 
optimal  guidance  path  is  then  converted  into  a  set  of  command  functions  and  fed  back 
into  the  control  computer  of  the  interceptor.  By  modifying  the  second  and  third 
derivatives  of  the  two-point  boundary  value  problem,  the  intercept  conditions  can  be 
varied  to  study  their  effects  upon  the  optimal  flight  path  regarding  the  maximization  of 
kinetic  energy  upon  impact. 


v 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


vi 


TABLE  OF  CONTENTS 


I.  INTRODUCTION . 1 

A.  BACKGROUND . 1 

B.  THESIS  ORGANIZATION . 3 

II.  MODELING  AND  SIMULATION . 5 

A.  TARGET  MODELING . 6 

1.  Basic  Definitions  and  Assumptions . 6 

2.  The  Ballistic  Missile  Model  Program . 8 

3.  Results . 12 

B.  INTERCEPTER  MODELING . 15 

1.  Basic  Definitions  and  Assumptions . 15 

2.  Interceptor  Missile  Model  Program . 18 

3.  Results . 21 

C.  HIGH-FIDELITY  MODELING . 22 

1.  Initialization . 23 

2.  The  Flight  Program . 28 

3.  Results . 39 

D.  COMMON  FUNCTIONS . 39 

1.  ZLDragC.m . 39 

2.  STatmos.m . 40 

III.  EXISTING  GUIDANCE  LAWS . 43 

A.  UNACCEPTABLE  GUIDANCE  LAWS . 43 

1.  Beam  Rider . 43 

2.  Pure  Pursuit . 43 

B.  LESS  THAN  OPTIMAL  GUIDANCE  LAWS . 44 

1.  True  Proportional  Navigation . 44 

2.  Compensated  Proportional  Navigation . 45 

3.  Augmented  Proportional  Navigation . 45 

C.  KEY  FATAL  CHARACTERISTICS . 46 

1.  Control  System  Time  Constant . 46 

2.  End-game  Environment . 47 

3.  Controlling  the  Interception  Geometry . 47 

IV.  PROPOSED  ALGORITHM . 49 

A.  PROBLEM  STATEMENT . 49 

B.  CALCULUS  OF  VARIATIONS . 50 

C.  PROGRAM  DEVELOPMENT . 55 

1.  Boundary  Conditions . 55 

2.  Separating  and  Recombining  Space  and  Time . 57 

3.  Reference  Trajectory . 58 

4.  Inverse  Dynamics . 60 

5.  Cost  and  Penalty  Functions . 61 

4.  Simulation  Outline . 62 

vii 


D.  SIMULATION  RESULTS . 64 

V.  CONCLUSIONS . 69 

A.  VERIFICATION  OF  OPTIMALITY . 69 

B.  APPLICABILITY . 78 

C.  SUGGESTIONS  FOR  FUTURE  RESEARCH . 78 

APPENDIX  A:  MODELING  PROGRAMS . 81 

A.  3DOF  TARGET . 81 

1.  BRParams3.m . 81 

2.  BRFlight3.m . 82 

B.  3D  OF  INTERCEPTOR . 85 

1.  SMParams3.m . 85 

2.  SMFlight3.m . 87 

C.  6DOF  MODEL . 92 

1.  BRDetails6.m . 92 

2.  BRParams6.m . 94 

3.  BRFlight6.m . 97 

3.  SMDetails6.m . 101 

5.  SMParams6.m . 103 

6.  SMFlight6.m . 106 

7.  ACoeff.m . 110 

8.  Animation.m . 112 

D.  COMMON  PROGRAMS . 117 

1.  ZLDragC.m . 117 

2.  STatmos.m . 117 

APPENDIX  B:  GUIDANCE  PROGRAMS . 121 

A.  DEMONSTRATION . 121 

B.  GUIDANCE  ALGORITHMS . 123 

1.  SMGuidance.m . 123 

2.  SMGuidanceCost.m . 129 

3.  SMTrajectory.m . 131 

LIST  OF  REFERENCES . 139 

INITIAL  DISTRIBUTION  LIST . 141 


viii 


LIST  OF  FIGURES 


Figure  1.  Engagement  Sequence  Groups  [From  Ref  9] . 1 

Figure  2.  Taep’o-dong  2  and  SM-6  size  comparison  (after  [Ref  9]) . 5 

Figure  3.  Ballistic  Missile  Flight  Path . 9 

Figure  4.  TD-2  Thrust  Generated  (Boost  Phase  Only) . 10 

Figure  5.  TD-2  Rocket  Mass  (Boost  Phase  Only) . 1 1 

Figure  6.  TD-2  Flyout  Range . 12 

Figure  7.  TD-2  Acceleration  and  Velocity  Profiles  (Entire  Flight) . 13 

Figure  8.  TD-2  Acceleration  and  Velocity  Profiles  (Boost  Phase  only) . 1 3 

Figure  9.  TD-2  Altitude  Profile  (Entire  Flight  and  Boost  Phase  only) . 14 

Figure  10.  SM-2  Configuration  Details  (Missile  Only) . 18 

Figure  1 1 .  SM-6  Interceptor  Thrust  Profile . 20 

Figure  12.  SM-6  Interceptor  Mass  (Boost  Phase  Only) . 21 

Figure  13.  Interceptor  Velocity  Profile . 22 

Figure  14.  Pitch  plane  stability  data . 33 

Figure  15.  Yaw  stability  and  control  derivatives . 34 

Figure  16.  Roll  control  derivatives . 34 

Figure  17.  Roll  control  Coupling  Derivatives . 35 

Figure  18.  Yaw  control  Derivatives . 35 

Figure  19.  Pitch  control  effects  on  roll  stability . 36 

Figure  20.  Pitch  control  derivatives . 36 

Figure  2 1 .  6DOF  Orientation  Animation . 39 

Figure  22.  Drag  Coefficient  by  Mach  Number  and  Flight  Phase . 40 

Figure  23.  Atmospheric  Temperature  Variation  by  Altitude . 41 

Figure  24.  Atmospheric  Density  by  Altitude . 41 

Figure  25.  Atmospheric  Pressure  Variation  by  Altitude . 42 

Figure  26.  Variation  of  Path  with  zf  and  x j"  (after  [Ref  18]) . 54 

Figure  27.  Variation  of  First  Derivative  of  Path  with  r  f  and  x'"0 . 54 

Figure  28.  X-axis  Flight  Path  and  Derivatives . 65 

Figure  29.  Y-axis  Flight  Path  and  Derivatives . 65 

Figure  30.  Z-axis  Flight  Path  and  Derivatives . 66 

Figure  31.  3D  Optimal  Flight  Path . 67 

Figure  32.  3D  Optimal  Flight  Path . 68 

Figure  33.  Final  Interception  Geometry . 69 

Figure  34.  2D  (X-Y  axis)  Optimum  Flight  Path . 70 

Figure  35.  2D  (X-Z  axis)  Optimum  Flight  Path . 70 

Figure  36.  2D  (Y-Z  axis)  Optimum  Flight  Path . 71 

Figure  37.  Heading  ( T1 )  and  Flight  Path  Angle  ( 0  )  Time  History . 72 

Figure  38.  Control  Forces  Time  History . 73 

Figure  39.  Penalty  Function  Values . 74 

Figure  40.  Iterative  Value  of  the  Cost  Function . 75 

Figure  4 1 .  Iterative  Value  of  the  Cosine  of  the  Impact  Angle . 76 


Figure  42.  Iterative  Value  of  Intercept  Time . 77 

Figure  43.  Iterative  Value  of  rf . 77 


x 


LIST  OF  TABLES 


Table  1.  Known  Infonnation  Regarding  the  TD-2 . 6 

Table  2.  Theoretical  Velocity  Capability  of  the  Target  Model . 8 

Table  3 .  Known  Infonnation  Regarding  the  SM-6 . 15 

Table  4.  Range  of  Possible  Interceptor  Thrust  Values . 17 

Table  5 .  WGS-84  Values . 24 

Table  6.  Frame  References  used  in  the  6DOF  Model  (After  [Ref  14]) . 25 

Table  7 .  Interceptor  Known  Data . 55 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


ACKNOWLEDGMENTS 


The  author  wishes  to  acknowledge  the  immense  patience  of  his  wife,  Sarah,  while 
he  spent  numerous  hours  in  the  computer  lab.  Her  unfailing  belief  in  my  abilities  often 
surpassed  my  own,  and  without  that  support  this  would  not  have  been  possible. 

The  author  also  wishes  to  acknowledge  the  assistance  of  his  advisor,  who 
patiently  answered  every  question,  no  matter  how  minor.  His  efforts  in  assisting  me  to 
troubleshoot  pages  and  pages  of  MATLAB  code  are  greatly  appreciated. 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


xiv 


I.  INTRODUCTION 


A.  BACKGROUND 

For  many  years  now,  the  United  States  Department  of  Defense  has  expended 
great  effort  to  develop  an  integrated  ballistic  missile  defense  system  through  a  layered, 
defense  in  depth  strategy.  A  ballistic  missile's  speed  and  altitude  leave  little  room  for 
error  by  the  defender,  and  any  strategy  must  include  systems  capable  of  defeating  a 
ballistic  missile  at  each  of  its  three  distinct  phases  -  boost,  midcourse,  and  tenninal  - 
which  the  Missile  Defense  Agency  labels  "Engagement  Sequence  Groups"  (ESG)  as 
shown  in  Figure  1  [Ref  9]. 


Figure  1 .  Engagement  Sequence  Groups  [From  Ref  9] 


It  is  the  portion  of  the  effort  directed  toward  the  boost  phase  of  the  Ballistic 
Missile  Defense  Programs  that  is  the  focus  this  paper.  The  boost  phase  ESG  is  concerned 
with  developing  methods  and  technologies  to  conduct  Boost  Phase  Intercept  (BPI). 
Intercepting  a  missile  in  its  boost  phase  is  the  ideal  solution  for  a  ballistic  missile  defense, 
since  the  missile  is  very  vulnerable  during  this  phase  of  its  flight.  The  missile  is  relatively 
slow  while  struggling  to  overcome  gravity,  has  a  very  visible  exhaust  plume,  and  cannot 
deploy  countermeasures  [Refs  9,  11].  Yet  the  challenges  needing  to  be  overcome  are 
immense:  countering  the  large  acceleration  rates,  reliable  scanning  and  tracking,  and  very 
short  reaction  time  being  the  most  daunting  [Refs  9,  11].  A  variety  of  weapon  systems 
are  under  development  for  conducting  boost  phase  interception,  including  airborne  lasers, 
space-based  intercept  missiles,  and  ground-based  intercept  missiles.  None  of  these 
systems  is  totally  operational,  though  several  look  promising. 
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This  paper  is  applicable  to  surface-based  interceptors,  specifically  the  United 
States  Navy's  Standard  Missile.  The  SM-6  very  quickly  reaches  its  top  speed  of  Mach  3+ 
[Ref  6,  12],  allowing  it  to  catch  up  and  maneuver  around  a  boosting  ballistic  missile.  The 
SM-6  is  not  currently  configured  for  missile  defense  options,  as  the  United  States  Navy 
has  not  yet  decided  on  appropriate  requirements  for  surface  ship  based  BPI  [Ref  6].  Yet, 
the  SM-6  missile  contains  the  necessary  capabilities  to  conduct  such  ballistic  missile 
defense  missions,  including  Advanced  Medium  Range  Air-to-Air  Missile  (AMRAAM) 
signal  processing  capabilities  that  enable  the  use  of  active  and  semi  active  radar, 
AMRAAM  guidance  and  control  systems,  and  advanced  fusing  techniques  [Ref  12]. 
Given  the  correct  guidance  laws  and  programming,  these  systems  combine  to  make  the 
missile  very  effective  out  to  the  maximum  kinetic  range.  This  paper  analyzes  one 
method  for  developing  such  a  guidance  law. 

A  missile’s  guidance  law  is  one  of  the  largest  single  factors  affecting  its  ability  to 
intercept  a  target.  Yet,  regarding  the  BPI  ESG,  intercepting  the  target  is  only  one  factor; 
the  other  major  consideration  is  the  ability  to  kill  the  target.  Early  ballistic  missile 
defense  concepts  recognized  that  a  simple  warhead  effect  is  not  sufficient  to  destroy  an 
ICBM  and  initiated  development  of  hit  to  kill  technologies  [Ref  2,  3].  The  relative  sizes 
of  a  nominal  ICBM  and  an  SM-6  means  that  the  interception  must  maximize  the  kinetic 
energy  transferred  to  the  ICBM  in  order  to  be  effective,  which  suggests  the  need  to 
control  the  geometry  of  the  interception.  Current  guidance  laws  do  not  address  this 
aspect,  leaving  the  actual  intercept  geometry  to  be  the  result  of  the  guidance  law  and  the 
relative  capabilities  of  the  missiles,  instead  of  an  input  into  the  guidance  law.  This  is  a 
reasonable  course  of  action  when  all  that  is  necessary  to  kill  the  target  is  to  get  the  missile 
within  the  limits  of  the  proximity  fuse,  the  case  with  most  surface-to-air  engagements.  It 
breaks  down,  however,  when  dealing  with  ballistic  missiles.  The  desire  for  hit-to-kill  end 
game  conditions,  coupled  with  the  need  to  maximize  the  kinetic  energy  transfer,  means 
the  interception  geometry  cannot  be  left  to  chance  and  must  be  controlled  as  an  input  of 
the  guidance  law. 

The  objective  of  this  thesis  is  to  design  a  guidance  law  that  will  generate  the 

interceptor’s  entire  flight  path  in  order  to  minimize  the  distance  traveled,  minimize  the 

time  to  intercept,  and  maximize  kinetic  energy  transfer  by  controlling  the  interception 
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geometry  while  providing  near-optimal  flight  path  to  interception.  This  will  be  done  by 
utilizing  the  direct  method  of  calculus  of  variations  combined  with  inverse  dynamics 
theory  to  reverse  engineer  in  real  time  an  optimal  flight  path  using  the  missile’s  onboard 
sensors  and  computers  [Ref  18]. 


B.  THESIS  ORGANIZATION 

Chapter  II  develops  the  simulation  models  for  the  ballistic  missile  target  and  the 
Standard  Missile  interceptor,  generating  a  mathematical  Three  Degree-of-Freedom 
(3DOF)  model  of  each.  This  paper  will  focus  on  the  Taep’o-dong  Two  (TD2)  ballistic 
rocket  in  development  by  the  People's  Democratic  Republic  of  Korea  (DPRK),  which  is 
believed  to  be  an  intercontinental  ballistic  missile  (ICBM)  capable  of  reaching  at  least 
Alaska  or  Hawaii  from  launch  sites  within  North  Korea  [Ref  4].  The  SM-6  is  the  US 
Navy's  latest  Extended  Range  Anti-Air  Warfare  missile  that  includes  Active-Homing 
Terminal  Guidance  -  the  key  feature  that  allows  for  the  accuracy  necessary  to  intercept 
an  ICBM  in  the  boost  phase.  Finally,  a  full  Six  Degree-of-Freedom  (6DOF)  model  is 
presented  for  future  research  to  capitalize  upon. 

Chapter  III  discusses  several  modem  guidance  laws,  describing  and  evaluating  for 
their  effectiveness  at  intercepting  and  killing  an  ICBM.  Two  families  of  guidance  laws, 
Pursuit  and  Proportional  Navigation,  are  examined.  The  reasons  for  the  necessity  of  a 
new  guidance  law  are  presented. 

Chapter  IV  develops  and  describes  the  new  guidance  law  and  test  program.  The 
guidance  law  is  continuously  calculated  onboard  the  missile  as  a  two  point  boundary 
value  problem,  using  Direct  Methods  of  Calculus  of  Variation  to  calculate  a  near-optimal 
flight  path  and  the  control  commands  necessary  to  achieve  it. 

Chapter  V  summarizes  the  results  and  discusses  the  feasibility  of  employing  such 
methods  in  a  real-world  scenario. 

The  Appendices  include  a  listing  of  all  MATLAB  functions  and  scripts  used  in 
the  simulation. 
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Throughout  the  remainder  of  this  paper,  the  tenn  "rocket",  “target”,  and 
"TD2"  will  be  used  to  refer  to  the  Taep’o-dong  2,  while  the  term 
"missile",  “interceptor”,  and  "SM-6"  will  refer  to  the  Standard  Missile  6. 


II.  MODELING  AND  SIMULATION 


This  chapter  develops  a  three-dimensional  target  model  that  operates  in  the 
Earth’s  gravitational  field,  using  the  TD-2  rocket  for  reference  data.  The  simulation 
models  a  two-stage,  boosting  target  that  reaches  intercontinental  velocities.  Then  a  three- 
dimensional  interceptor  model  is  developed  that  operates  in  the  Earth’s  gravitational 
field,  using  the  SM-6  missile  for  reference  data.  The  simulation  models  a  two-stage, 
boosting  missile  that  reaches  nominal  velocities.  Figure  2  shows  a  comparison  of  the 
relative  sizes  of  the  missiles  involved. 


The  SM-6 
is  roughly 
%  the  size 
of  the  TD-2 


Figure  2.  Taep’o-dong  2  and  SM-6  size  comparison  (after  [Ref  9]) 
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A.  TARGET  MODELING 

1.  Basic  Definitions  and  Assumptions 

Table  1  shows  the  basic  details  of  a  Taep’o-dong  two  (TD-2)  rocket  [Ref  4]. 


Overall 

Length 

32  m 

Payload 

750-1000  kg 

Range 

3500-4300  km 

Stages 

2 

Thrust 

Chambers 

4,1 

Type 

LR ICBM 

Stage  1 

Stage  2 

Diameter 

2.2  m 

1.335  m 

Length 

16  m 

14  m 

Launch  Weight 

-60,000  kg 

15,200  kg 

Thrust 

-103,000  kgf 

13,350  kgf 

Fuel  /  Oxidizer 

TM-185 /AK-27I 

TM-185 /AK-27I 

Propellant  Mass 

- 

12,912  kg 

Bum  Time 

-125  s 

110  s 

Table  1.  Known  Information  Regarding  the  TD-2 


The  limited  data  must  be  extrapolated  into  a  complete  missile  picture.  This 
required  several  assumptions,  which  will  be  discussed  during  the  course  of  the 
extrapolation. 

The  first  assumption  was  that  of  a  linear  ratio  between  the  fuel  and  the  thrust. 
The  thrust  developed  was  assumed  to  be  a  weak  function  of  fuel  consumption  and  the 
number  of  thrust  chambers,  and  a  strong  function  of  the  specifics  of  the  engines.  Stage 
one  has  four  chambers  while  stage  two  has  one  chamber;  therefore  the  ratio  of  the  fuel 
consumption  rates  should  not  be  greater  than  4.  The  total  propellant  mass  of  stage  two  is 
12,912  kg  while  the  total  mass  of  the  stage  is  15,200  kg,  for  a  fuel  mass  fraction  of  0.849 
and  a  fuel  consumption  rate  (for  110  s)  of  117.38  kg/s.  This  value  is  appropriate  for  a 
ballistic  missile  [Ref  21],  Assuming  the  same  ratio  for  stage  one  results  in  a  fuel  mass  of 
50,970  kg  and  a  fuel  consumption  rate  (for  125  s)  of  407.8  kg/s. 

The  fuel  used  is  a  combination  of  a  liquid  fuel  and  a  liquid  oxidizer.  TM-185  is 
composed  of  20%  Gasoline  (737.22  kg/m3)  and  80%  Kerosene  (817.15  kg/m3)  [Ref  4], 
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which  results  in  a  fuel  density  of  801.164  kg/m3.  The  oxidizer,  AK-27I,  is  composed  of 
27%  N204  (1,450  kg/m3)  and  73%  HN03  (1,580  kg/nr3)  [Ref  4],  which  results  in  an 
oxidizer  density  of  1,544  kg/m  .  Finally,  the  fuel  to  oxidizer  ratio  (F/O)  for  this  fuel 
combination  is  4.05:1  [Ref  16],  yielding  a  cumulative  density  of  1,182.62  kg/m  .  Given 
the  density  of  the  fuel,  the  total  volume  for  each  stage  is  easily  determined.  For  stage  one 
the  required  volume  of  fuel  is  43.1  m  ,  while  for  stage  two  the  required  volume  of  fuel  is 
10.9  m  .  Since  the  total  available  volume  of  stage  one  is  60.82  m  ,  and  the  total  available 
volume  of  stage  two  is  19.6  m  ,  the  values  are  reasonable. 

Given  that  both  stages  use  the  same  fuel,  the  specific  impulse  (Isp)  should  be  the 
same  for  both  stages,  which  is  expressed  as  [Ref  21] 


T_ 

W 


(2.A.1) 


where  W  is  the  in-stage  fuel  consumption  rate  (in  kg/s)  and  T  is  the  thrust  produced.  The 
first  stage  Isp  is  252.57  s,  which  is  reasonable  for  an  intercontinental  ballistic  missile.  Yet 
the  second  stage  Isp  using  the  given  thrust  data  is  only  113.73  s,  which  is  too  low  to 
accelerate  the  rocket  to  intercontinental  speeds  [Ref  21],  The  second  stage  Isp  will 
therefore  be  assumed  to  be  252.57  s,  and  the  resultant  thrust  is  then  29,650  kgf. 


Under  these  assumptions,  it  is  possible  to  determine  the  resultant  increase  in 
velocity  AV  (in  m/s)  by  the  rocket  equation  [Ref  21] 


AF„  =  7  gin 


'  1  A 


where  m  f  n  =  — 


m , 


(2.A.2) 


Tjmt,i+mpay 


where  n  is  the  stage  number,  mP:„  (in  kg)  is  the  stage  propellant  mass,  m,A  (in  kg)  is  the 
total  stage  mass,  and  mpay  (in  kg)  is  the  payload  mass.  In  each  stage,  all  masses  except 
the  propellant  mass  in  that  stage  is  considered  part  of  the  structural  mass.  Each  of  the 
stages  yields  a  separate  AV  where  the  total  velocity  capability  of  the  overall  system  is 

AV  =  ]TAVi  (2.A.3) 

1=1 

where  n  is  the  number  of  stages. 
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This  value  does  not  account  for  drag  or  the  variation  of  gravity,  so  it  is  only  a 
theoretical  estimate  of  the  final  value  that  will  be  used  to  verify  the  model  is  working 
correctly.  The  theoretical  values  are  listed  in  Table  2. 


Stage  1 

Stage  2 

Total 

Stage  Mass  Fraction 

0.671 

0.809 

- 

AV  (in  m/s) 

2755.21 

4108.68 

6863.90 

Table  2.  Theoretical  Velocity  Capability  of  the  Target  Model 


The  final  velocity  from  the  model  should  therefore  be  less  than  6863  m/s,  since 
both  gravity  and  drag  will  be  working  against  the  launch. 

2.  The  Ballistic  Missile  Model  Program 

The  3DOF  model  presented  here  is  a  series  of  MATLAB  functions  on  a  repeating 
integration  loop,  using  four  function  files  to  accomplish  the  modeling  (the  “3”  at  the  end 
of  each  title  refer  to  the  3DOF  model). 

1.  BRFlight3.m  -  integrates  each  time  step  to  determine  the  current  position, 
attitude,  and  aerodynamic  forces  acting  on  the  rocket/missile; 

2.  BRParams3.m  -  determines  the  mass  of  the  rocket  and  the  surface 
reference  area; 

3.  ZLDragC.m  -  detennines  the  drag  coefficient  (described  in  section  D); 

4.  STatmos.m  -  determines  the  properties  of  the  local  atmosphere  (described 
in  section  D); 

The  program  BRFlight3.m  generates  a  ballistic  flight  path  that  will  be  intercepted 
by  the  SM-6  based  on  the  model  developed  by  Zarchan  [Ref  21].  The  main  difference  is 
that  Zarchan  developed  a  two-dimensional  x-y  model,  whereas  this  paper  requires  a 
three-dimensional  model.  The  mapping  to  a  three  dimensional  system  is  done  simply  by 
employing  the  x-y  equations  as  x-z  equations  and  making  the  y-values  a  constant,  in  this 
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case  zero.  Thus  a  three-dimensional  flight  path  is  created  entirely  contained  within  the  x- 
z  plane  as  shown  in  Figure  3,  where  the  asterisks  represent  the  staging  events. 


Y  position  (m)  X  position  (m) 

Figure  3.  Ballistic  Missile  Flight  Path 


The  launch  position  is  at 

x0  =0 

To  =  0  (2.A.4) 

z0  =  Re 

where  Re  is  the  WGS-84  radius  of  the  Earth,  6,378,137  m. 

The  initial  velocities  are: 

x0=V0cos6lcosvF 

y0=V0cos6,sin'P  (2. A. 5) 

z0=V0sin# 

where  Vo  is  the  initial  velocity,  6  is  the  initial  elevation  angle,  and  T  is  the  initial 
heading.  A  heading  of  T  =  0  will  result  in  an  initial  y  velocity  of  0  as  required  for  this 
model.  The  launch  angle,  6 ,  is  85  degrees,  which  was  chosen  to  maximize  the  range 
while  still  recognizing  the  restrictions  on  launching  such  a  large  missile  as  the  TD-2  (45- 
60  degrees  is  not  a  feasible  launch  angle)  [Ref  21], 

The  program  first  calculates  the  axial  force  on  the  missile,  aT ,  which  is  based  on 

the  thrust  and  drag  forces  acting  on  the  missile.  The  thrust  is  a  given  set  of  time-based 
values  based  on  the  previously  articulated  known  data  and  assumptions,  shown  in  Figure 
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4.  The  thrust  also  drops  sharply  at  130  seconds  and  240  seconds  to  represent  the  staging 
events.  Following  the  completion  of  the  boost  phase,  the  thrust  is  zero,  though  the  axial 
force  is  not  due  to  the  continuous  presence  of  drag. 

12 
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TD-2  Thrust  Generated  (Boost  Phase  Only) 

The  drag  requires  several  steps  to  calculate,  including  determination  of  the  local 
atmospheric  density  and  temperature  (using  the  function  STatmos.m),  the  atmospheric 
drag  constant  (using  the  function  ZLDragC.m),  and  the  reference  surface  area  (using  the 
function  BRParams3.m).  STatmos.m  and  ZLDragC.m  are  described  in  section  D  of  this 
chapter. 

The  rocket’s  mass  is  a  simple  function  of  time  (using  the  function  BRParams3.m), 
shown  in  Figure  5.  The  mass  drops  sharply  at  130  seconds  and  at  240  seconds,  which 
represent  the  staging  events.  After  the  completion  of  the  boost  phase,  the  mass  remains 
constant  for  the  duration  of  the  flight. 


H 


Figure  4. 
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Time  (sec) 

Figure  5.  TD-2  Rocket  Mass  (Boost  Phase  Only) 


The  axial  thrust  force  is  then  the  difference  between  the  thrust  and  drag,  and  the 
axial  acceleration  is  given  by 


(T-D) 

mg 


(2.A.6) 


Using  the  nominal  two-stage  booster  design,  a  simple  gravity  turn  is  generated  by 
aligning  the  thrust  vector  with  the  velocity  vector,  keeping  in  mind  the  flight  path  is 
entirely  contained  within  the  x-z  plane  [Ref  21] 

..  _  -gm  x  aTx 

X  =  (x2  +  z2)L5+(i2+i2)0'5 

v  =  0  (2.A.7) 

..  _  -gm  z  aTz 

Z~  (x2+z2f5  +  (x2+z2f 5 

where  gm  is  the  WGS-84  Earth’s  gravitational  constant,  3.986xl014  m3/s2  [Ref  14]. 
Equations  (2.A.7)  are  integrated  for  the  duration  of  the  rocket  flight. 
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3.  Results 

The  two  dimensional  graph  of  the  x-z  plane,  shown  in  figure  6,  clearly  shows  the 
altitude  and  range  of  the  rocket,  which  compares  favorably  with  the  known  data 
presented  earlier  and  Zarchan  [Ref  21],  where  the  asterisks  again  represent  the  location  of 
the  staging  events.  As  noted  in  Zarchan,  the  flat  earth  equations  used  here  are  only 
moderately  accurate  over  the  course  of  the  rocket’s  entire  flight,  but  since  the  focus  of 
this  paper  is  only  on  the  boost  phase,  the  accuracy  of  the  termination  position  is 
irrelevant.  During  the  boost  phase  the  accuracy  of  the  flat  earth  equations  is  very  good. 


Figure  6.  TD-2  Flyout  Range 

The  acceleration  required  to  achieve  these  ranges  is  on  the  order  of  6.5  km/s  [Ref 
21].  Figures  7  and  8  clearly  show  that  this  speed  has  been  achieved  at  the  end  of  the 
boost  phase,  and  thus  the  range  values  are  appropriate.  Figure  7  shows  the  velocity 
profile  for  the  entire  flight.  The  rocket  reaches  a  velocity  of  nearly  6  km/s  at  the  end  of 
the  boost  phase.  After  burnout  it  decelerates  due  to  gravity  until  it  reaches  its  apogee, 
after  which  point  it  begins  to  accelerate  due  to  gravity. 
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Figure  7.  TD-2  Acceleration  and  Velocity  Profiles  (Entire  Flight) 


Figure  7  shows  a  closer  look  at  the  boost  phase  acceleration  and  velocity  profiles. 
The  effects  of  staging  are  readily  apparent.  It  is  clear  that  the  values  are  consistent  with, 
but  slightly  less  than,  the  predictions  from  Table  2,  as  expected  due  to  the  presence  of 
gravity  and  drag. 


Figure  8.  TD-2  Acceleration  and  Velocity  Profiles  (Boost  Phase  only) 
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The  determination  of  the  available  time  for  a  surface-launched  missile  to  intercept 
the  target  is  one  of  the  critical  values  that  can  now  be  determined.  Figure  9  shows  the 
altitude  profile  for  the  TD-2  for  the  entire  flight  and  the  boost  phase  only.  The  effects  of 
acceleration  are  quite  apparent.  The  uppermost  limit  of  the  atmosphere  according  to  the 
WGS-84  standard  atmospheric  model  is  86  km.  Even  at  86  km,  however,  an 
endoatmospheric  missile  has  a  hard  time  maneuvering  due  to  the  low  density  of  the  local 
atmosphere.  Thus  the  maximum  allowable  intercept  value  must  be  lowered;  in  this  case 
50-60  km  will  be  considered  the  upper  limit.  The  target  achieves  this  altitude  between 
130  s  and  140  s.  This  is  one  of  the  most  significant  limitations  on  the  BPI  problem,  since 
a  US  Navy  ship  on  station  and  actively  monitoring  the  launch  area  will  still  need  45-60 
seconds  to  detect,  track,  analyze,  and  engage  the  target.  This  paper  will  assume  a  60 
second  delay  in  the  interceptor  launch. 


0  50  100  150  200  250 

Time  (sec) 


Figure  9.  TD-2  Altitude  Profile  (Entire  Flight  and  Boost  Phase  only) 
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The  final  step  of  the  program  is  to  record  all  the  data  for  the  rocket.  This  data  will 
be  called  by  the  interceptor  simulation  to  mimic  the  missile’s  onboard  sensors.  The 
missile  will  “see”  the  location  and  velocity  of  the  rocket  at  the  appropriate  intervals  by 
coordinating  the  launch  time  of  the  interceptor  with  the  launch  time  of  the  ballistic 
missile. 

B.  INTERCEPTER  MODELING 

1.  Basic  Definitions  and  Assumptions 

The  following  are  the  basic  details  of  a  Raytheon  Standard  Missile  6  (SM-6) 


[Refs  6,  12]. 


Overall 

Length 

6.5  m 

Payload 

115  kg 

Range 

150  km 

Stages 

2 

Thrust 

Chambers 

U 

Type 

ERAAW 

Stage  1 

Stage  2 

Diameter 

0.53  m 

0.34  m 

Length 

1 .72  m 

4.78  m 

Launch  Weight 

712  kg 

686  kg 

Thrust 

- 

- 

Fuel  /  Oxidizer 

HTPB-AP 

TP-H1205/6 

Propellant  Mass 

468  kg 

360  kg 

Bum  Time 

6  s 

- 

Table  3.  Known  Information  Regarding  the  SM-6 


Again,  the  limited  data  must  be  extrapolated  into  a  complete  missile  picture. 
Several  assumptions  were  again  made,  which  will  be  discussed  during  the  course  of  the 
extrapolation. 

The  two  stages  have  dissimilar  fuels,  so  very  few  assumptions  can  be  correlated 
between  the  two.  One  assumption  that  can  be  made  and  applied  to  both  solid  propellants 
is  the  grain  pattern.  It  has  been  assumed  that  both  motors  use  a  star  grain  pattern,  which 
was  designed  to  and  is  known  to  very  effectively  provide  a  constant  stable  burn 
throughout  the  flight.  A  star  grain  pattern  produces  thrust  variations  of  less  than  4%  for 
the  duration  of  the  burn  [Ref  10].  This  grain  pattern  typically  has  a  volume  loading 
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fraction  of  60-80%,  which  will  be  the  assumed  range  for  both  stages.  The  final  value  will 
be  detennined  by  what  gives  a  reasonable  value  for  structural  thickness. 

The  Stage  1  fuel  is  HTPB-AP.  The  presence  of  smoke  during  launch,  in  addition 
to  the  massive  thrust  required,  strongly  suggests  the  presence  of  a  metal  (probably 
aluminum).  The  density  of  the  combination  of  those  three  components  that  yields  the 
highest  specific  impulse  is  1860  kg/nr.  A  fuel  with  a  mass  of  468  kg  with  a  density  of 
I860  kg/m  has  a  volume  of  0.25  m  .  Applying  the  loading  fraction  of  60%  to  the 
volume  and  subtracting  from  the  total  volume  of  the  Mk-72  engine  leaves  0.0208  m  (-4/5 
in)  average  thickness  for  the  structural  components.  This  will  be  the  assumed  average 
value  for  the  external  structure  of  the  SM-6. 

When  applied  to  Stage  2,  the  density  of  the  TP-H1205/6  fuel  is  roughly  3000 
kg/m  ,  which  is  too  high.  Assuming  a  volumetric  fraction  of  80%  yields  a  density  of 
2267  kg/m  ,  a  more  realistic  value. 

Solid  fuel  motors  used  aboard  U.S.  Navy  ships  must  have  a  Department  of 
Defense  (DOD)  Hazard  Classification  of  1.1  or  1.3.  Typical  solid  rocket  fuels  of  this 
category  have  a  specific  impulse,  Isp,  in  the  range  of  180-270  seconds  [Ref  3].  The  thrust 
produced  by  such  a  rocket  motor  is  given  by 

F  =  Ispmg  (2.B.1) 

cItyi 

where  m  is  the  change  in  mass  over  time  or  the  stage  fuel  consumption  rate  — (in  kg/s) 

dt 

and  g  is  the  gravitational  acceleration  at  the  current  distance  from  the  center  of  the  Earth 
(in  m/s").  Using  the  known  masses  and  burntimes,  and  assuming  a  fifteen  second 
bumtime  for  the  second  stage,  results  in  the  range  of  possible  thrust  values  given  in  Table 
4.  Since  open  source  literature  details  the  SM-3  (which  uses  the  same  engines)  speed 
capability  as  4000  m/s  [Ref  6],  the  final  implemented  values  will  be  chosen  to  accelerate 
the  SM-6  to  that  speed  in  a  reasonable  time. 
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Stage  1 

Stage  2 

Mass  (kg) 

468 

360 

Burntime  (s) 

6 

15 

Thrust  Range  (N) 

137,732.4-206,598.6 

42,379.2-63,568.8 

Table  4.  Range  of  Possible  Interceptor  Thrust  Values 


The  density  of  the  structural  components  must  be  calculated  by  omission,  since 
the  assumption  can  be  made  that  the  material  is  some  form  of  composite  vice  anything 
like  common  structural  steel.  The  volume  of  each  component  was  calculated  assuming 
simple  geometric  shapes  -  tangent  ogive  nosecone,  cylindrical  body  sections,  trapezoidal 
wingforms,  and  triangular  tail  control  surfaces.  No  detailed  specifications  are  available 
for  the  SM-6,  but  it  is  similar  enough  to  the  SM-2,  shown  in  Figure  10  [Ref  1],  for  the 
purposes  of  calculating  the  sizes  of  structural  components.  An  additional  modifying 
assumption  to  account  for  the  sensor  and  computer  equipment  inside  the  radome  was 
included  by  adding  30%  of  the  volume  of  the  radome  as  structural  weight.  The  total 

-3 

volume  of  structural  components  was  0.162  m  ,  and  the  total  weight  of  structure  and 
miscellaneous  components  was  685  kg  (everything  except  fuel  and  warhead),  resulting  in 

•3 

a  structural  density  of  4220  kg/m  ,  which  is  a  realistic  value  for  a  high  strength  composite 
material. 
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STA  0 


Figure  10.  SM-2  Configuration  Details  (Missile  Only) 


2.  Interceptor  Missile  Model  Program 

The  3DOF  model  presented  here  is  a  series  of  MATLAB  functions  on  a  repeating 
integration  loop,  using  four  function  files  to  accomplish  the  modeling,  (the  “3”  at  the  end 
of  each  title  refer  to  the  3DOF  model). 

1.  SMFlight3.m  -  integrates  each  time  step  to  determine  the  current  position, 
attitude,  and  aerodynamic  forces  acting  on  the  rocket/missile; 

2.  SMParams3.m  -  detennines  the  mass  of  the  rocket  and  the  surface 
reference  area; 

3.  ZLDragC.m  -  determines  the  drag  coefficient  (described  in  section  D); 
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4.  STatmos.m  -  determines  the  properties  of  the  local  atmosphere  (described 
in  section  D); 

The  program  SMFlight.m  generates  the  flight  plan  using  the  axial  velocity,  V,  the 
heading  angle,  T  ,  and  the  flight  path  angle,  0 ,  related  through  the  kinematic  equations 
[Ref  19] 

x  =  V  cos  0  cos  W 

y  =  V  cosOsinW  (2.B.2) 

z  =  V  sin0 

The  three  components  are  the  result  of  the  forces  acting  on  the  missile,  related 
through  the  dynamics  equations  [Ref  19] 

V  =  g  (nx  -  sin  6) 

e  =  f  (ny-  cosO)  (2.B.3) 

¥  = — ^ — n_ 

V  cos  6 

where  nx  is  the  axial  force,  nv  is  the  yaw  force,  and  n,  is  the  lift  force. 

Further,  the  acceleration  components  are  found  from  the  derivatives  of  equation 

(2.B.2). 

Xj  =  V  cos0cos'F-0F  sin0cosT/-xFF  cos^sinT 

x2  =  V  cos  0  sin  -0V  sin  6  sin  'F  +  'F  V  cos  6  cos  'F  (2.B.4) 

x3  =  F  sin  9 +  9V  cos  9 

The  axial  forces  are  calculated  in  the  same  manner  as  the  axial  force  of  the 
ballistic  missile,  in  fact  re-using  both  the  STatmos.m  and  ZLDragC.m  functions. 

STatmos.m  and  ZLDragC.m  are  described  in  section  D  of  this  chapter.  The 

SMParams.m  function  uses  the  same  methodology  as  the  BRParams.m  function  to 
calculate  the  reference  surface  area  and  the  mass. 
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The  thrust  is  a  given  set  of  time-based  values  based  on  the  known  data  and  the 
assumptions,  shown  in  Figure  11.  The  thrust  drops  sharply  at  6  s  and  26  s,  again  to 
represent  the  staging  events. 
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Figure  1 1 .  SM-6  Interceptor  Thrust  Profile 

The  mass  is  again  a  simple  function  of  time  (using  the  function  SMParams.m), 
shown  in  Figure  12.  The  mass  drops  sharply  at  6  seconds  and  at  26  seconds,  again  to 
represent  the  staging  events. 
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Figure  12.  SM-6  Interceptor  Mass  (Boost  Phase  Only) 


The  axial  thrust  force  is  then  the  difference  between  the  thrust  and  drag,  and  the 
axial  acceleration  is  given  by 


n 


x 


( T-D ) 

mg 


(2.B.5) 


The  remaining  two  forces ,n  ,n_,  are  returned  from  the  guidance  laws  and  applied  by 
integrating  equations  (2.B.3). 


3.  Results 

Many  of  the  results  are  specifically  dependant  on  the  guidance  law,  but  several 

are  relatively  independent,  such  as  the  Velocity,  Acceleration,  and  generic  flight  profile. 

The  Isp  of  both  stages  was  maximized  but  still  unable  to  accelerate  the  SM-6  beyond  a 

speed  of  3,500  m/s.  This  speed  was  not  detrimental  to  the  simulation  and  was  determined 

to  be  the  most  accurate  value  considering  all  the  data.  Figure  13  shows  the  generic 
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Velocity  and  Acceleration  profile  produced  by  the  model,  which  will  change  slightly  with 
each  run  due  to  the  guidance  law  in  effect  and  specifics  of  the  simulation. 


Figure  13.  Interceptor  Velocity  Profile 

C.  HIGH-FIDELITY  MODELING 

Although  the  guidance  law  modeled  and  developed  in  sections  III  and  IV  deals 
with  a  3DOF  models  discussed  earlier,  the  follow-on  research  will  require  a  higher 
fidelity  model,  such  as  a  Six-Degree-of-Freedom  (6DOF)  model  of  the  interceptor  missile 
presented  here.  This  model  uses  the  same  fundamental  algorithm  for  the  guidance  law, 
but  instead  of  treating  the  interceptor  as  a  point  mass  and  considering  only  position, 
velocity,  and  acceleration,  it  will  also  consider  attitude  and  orientation.  It  also  goes  one 
step  further  than  the  algorithm  presented  by  converting  acceleration  commands  into 
aileron  commands. 

The  higher  fidelity  modeling  was  also  addressed  in  this  study.  The  6DOF  model 
presented  here  is  a  series  of  MATLAB  functions  on  a  repeating  integration  loop.  It  uses 
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seven  function  files  to  accomplish  the  modeling,  each  applying  to  their  respective 
simulations  (the  “6”  at  the  end  of  each  title  refer  to  the  6DOF  model).  The  programs  are 
very  similar,  only  the  control  inputs  and  physical  parameters  differ. 

1.  BRInitialize6.m  and  SMInitialize6.m,  which  initializes  the  first  step  values 
based  on  launch  conditions; 

2.  BRDetail6.m  and  SMDetail6.m,  which  describes  the  rocket/missile 
characteristics  such  as  structural  thickness,  sizes,  and  fuel  weights,  etc. 

3.  BRFlight6.m  and  SMFlight6.m,  which  repeats  for  each  time  step  to 
determine  the  current  position,  attitude,  and  aerodynamic  forces  acting  on 
the  rocket/missile; 

4.  BRParams6.m  and  SMParams6.m,  which  detennines  the  mass  of  the 
rocket/missile  and  moment  of  inertia  based  on  time,  and  several 
aerodynamic  derivatives; 

5.  AeroC.m,  which  interpolates  to  determine  several  aerodynamic  values 
based  on  angle  of  attack,  altitude,  and  control  surface  deflections; 

6.  ZLDragC.m; 

7.  STatmos.m; 

The  output  of  the  model  is  utilized  by  the  guidance  law  program,  SMGuidance.m, 
to  determine  the  necessary  inputs  to  the  interceptor  missile  auto-pilot. 

1.  Initialization 

In  order  to  increase  the  accuracy  of  the  simulation,  the  Earth  is  NOT  assumed  to 
be  a  perfect,  non-rotating  spheroid.  Based  on  the  World  Geographic  Survey  of  1984,  the 
following  values  were  used  in  the  program  [Ref  14]. 


23 


Earth's  Radius,  Re 

6,378,137  m 

Earth's  Semi-Minor  Axis,  b 

6,356,752  m 

Earth's  Flattening  (1/Ellipticity),/ 

1/298.257223563 

Earth’s  Rotation  Rate  (Qze) 

7.2921 16  e-5  rad/sec 

Earth's  Gravitational  Constant,  GM 

3. 9860044 18e  14  m3/s2 

Table  5.  WGS-84  Values 


A  launch  point  was  needed  from  North  Korea,  and  Pyongyang  was  chosen 
arbitrarily.  The  coordinates  for  the  launch  point  (pi,  Xi)  are  (40°54’  North,  129°34'  East). 
A  suitable  US  city  was  needed  within  the  maximum  estimated  range  of  the  TD-2  missile, 
3,500-4,000  km,  so  Pearl  Harbor,  at  4,260  km  at  a  bearing  of  010°,  was  selected.  The 
coordinates  for  the  target  point  (pt,  Xt)  are  22°03’  North  by  159°09’  West  [Ref  5],  The 
geocentric  launch  latitude  is  found  by  converting  from  the  geodetic  latitude  according  to 
[Ref  8,  14] 

tan  jus  =  (1  -  /)2  tan  jug  (2.C.  1) 

The  coordinates  of  the  launch  point  were  converted  from  Geodetic  into 
Rectangular  coordinates  according  to  [Ref  14] 

Rs  =  — 7  fe.  ,  =  ,  where  e  =  J- - — >  (2.C.2) 

yj l-e2sin  jus  V  a 

and 
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p(  0)  = 


(2.C.3) 


pM 

Rs  cos  A 

Py(0) 

= 

0 

_pA°)_ 

Rs  sin  fj,s 

The  initial  velocity  was  defined  as 


v(o)  = 


0 

0 

0 


(2.C.4) 


Several  different  coordinate  systems  are  involved  in  the  flight  of  the  interceptor 
missile.  The  propulsive  forces  act  on  the  missile  at  its  center  of  gravity,  while  the 
aerodynamic  forces  act  relative  to  the  movement  of  the  missile  with  respect  to  the 
atmosphere,  and  the  gravitational  forces  depend  on  the  position  of  the  missile.  In  order  to 
apply  all  forces  equally  and  appropriately  it  is  necessary  to  define  convenient  coordinate 
systems  for  each  and  then  rotate  those  coordinate  systems  into  a  common  one.  Five 
reference  frames  will  be  utilized  in  the  model:  two  geocentric;  two  geographic;  and  a 
body  fixed  (subscripted  b). 


Frame  of  Reference 

Coordinate  System 

Geocentric 

Fit  an  "inertial  frame",  non-rotating  but 
translating  with  Earth's  cm 

ECI  (Earth  Centered  Inertial),  origin  at 
Earth's  cm,  axes  in  the  equatorial  plane  and 
along  the  spin  axis 

Geocentric 

Fe ,  a  frame  defined  by  the  "rigid"  Earth 

ECEF  (Earth-centered,  Earth-fixed),  origin 
at  Earth's  cm,  axes  in  the  equatorial  plane 
and  along  the  spin  axis 

Geographic 

Tangent-plane  system,  a  geographic  system 
with  its  origin  on  the  Earth's  surface 

Geographic 

F ned  (also  F enu)>  a  frame  translating 
with  the  vehicle  cm,  in  which  the  axes 
represent  fixed  directions 

Vehicle-carried  system,  a  geographic 
system  with  its  origin  at  the  vehicle  cm 

Body 

F b ,  a  "body"  frame  defined  by  the  "rigid" 
vehicle 

Vehicle  body-fixed  system,  origin  at  the 
vehicle  cm,  axes  aligned  with  vehicle 
reference  directions 

Table  6.  Frame  References  used  in  the  6DOF  Model  (After  [Ref  14]) 
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The  initial  reference  frame  is  Earth  Centered  Inertial  (F,).  This  frame  includes  the 
rotation  of  the  earth  as  a  translational  motion.  The  earth's  rotation  can  be  removed  from 
the  equation,  thereby  simplifying  the  rest  of  the  problem,  by  rotating  the  frame  about  the 
z-axis  through  the  angle 

ju-^o  -Llong  +  WzEt  (2.C.5) 

The  celestial  longitude,^,  is  arbitrary  and  can  be  chosen  to  be  zero.  The  resulting 
rotation  matrix  is  [Ref  8,14] 

cos  {ILong  +  WzEt )  sin(lLong  +  WzEt)  0 

Re/i=  -sin  (ILong +  WzEt)  cos  (ILong +  WzEt)  0  (2.C.6) 

0  0  1 

where  e  and  i  are  used  to  designation  ECI  and  ECEF  systems,  respectively. 

The  system  must  now  be  rotated  into  the  navigational  reference  system,  normally 
designated  North-East-Down  for  the  directions  that  x-y-z  point,  respectively.  However, 
and  intermediate  step  is  required  to  rotate  from  ECEF  to  Up-East-North  first.  This  is 
accomplished  by  rotating  first  through  the  geocentric  longitude, 

cos  {latgc)  0  sin  {latgc) 

Rule  =010  (2.C.7) 

-sin  (latgc)  0  cos  {latgc) 

then  through  the  v-axis  to  point  the  x-axis  North  and  the  z-axis  Down, 

“0  0  1" 

R„/u=  0  1  0  (2.C.8) 

-10  0 

In  order  to  rotate  the  NED  system  into  the  body-fixed  system,  the  initial  roll  {(p), 
pitch  ( 6 ),  and  yaw  (i//)  angles  must  be  defined.  The  initial  roll  is  defined  as  zero,  as  the 
missile  is  not  rotating  about  its  x-axis  on  the  launch  pad.  The  initial  pitch  is  the  elevation 
of  the  launch  direction  with  respect  to  the  horizon.  The  initial  yaw  is  the  initial  heading 
with  respect  to  north.  With  these  terms  defined,  the  rotation  matrices  for  each  variable 
are  [Ref  22] 
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"1  0 

0 

R*= 

0  cos  (j> 

sin^ 

0  -sin^  cos  <f> 

cos  #  0  -sin# 

Re  = 

0  1 

0 

o 

_ i 

cos# 

cos^ 

sin  y  0 

K  = 

-sin^  cos^  0 

0 

0  1 

and  the  total  rotation  from  NED  to  body-centered  system  is 

Rb/n=R+RgR¥  (2.C.10) 

The  final  complete  rotational  transformation  is  the  combination  of  all  the  rotations 

Rb/i  =  Rb/nRn/uRu/eRe/i  P.C.ll) 


This  rotation  matrix  allows  for  the  computation  of  several  important  variables, 
notably  the  angular  velocity  and  the  Euler  angles  which  will  lead  directly  to  the 
Quaternion  of  this  system. 

The  initial  angular  velocity  is  found  by  transforming  the  Earth's  rotation  into  the 
body  system  according  to  [Ref  8] 


co\  =  Rb/i 


0 

0 

WzK 


(2.C.12) 


It  is  often  preferable  to  track  the  system  orientation  through  the  use  of  the 
Quaternion  instead  of  using  other  methods  such  as  Euler  Kinematic  Equations  or  Poisson 
Kinematic  Equations.  The  initialization  of  the  Quaternion  is  derived  from  the  initial 
Euler  angles.  The  Euler  angles  are  defined  from  the  rotation  matrix  Rb/i  (a  3x3  Matrix) 
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(j)E  =  tan 


-1 


3) 

v^/,(3,3)y 


^ =-sin  H^cu)) 


(//,.  =  tan 


-1 


(2.C.13) 


The  initial  Quaternion  is  calculated  from  Euler  angles  [Ref  14] 

%  =  cos(^)  cos(^)  cos(^)  +  sin(^)  sin(^)  sin(^) 
<7i  =  sin(^)  cos(^)  cos(^)  -  cos(^)  sin(^)  sin(^) 
q2  =  cos(^)  sin(^)  cos(^)  +  sin(^)  cos(^)  sin(^) 
q3  =  cos(^)  cos(^)  sin(^)  -  sin(^)  sin(^)  cos(^) 


(2.C.14) 


To  check  the  validity  of  the  Quaternion,  the  rotation  matrix  Rb/i  can  be  re¬ 
evaluated  [Ref  14] 


Kn 


ial 


2  2  \ 

q2-q3) 

2(q]q2-q(jq3) 

2(qiqi+q0q2) 


2(qtq2+q(lq3) 

ial  -Vi  +til  -vl) 

2(q2q3-qQqx) 


2(?l?3-?0?2) 

2(^3 +q0qi) 

(q0  -Qi _  ^2  ^3 ) 


(2.C.15) 


The  results  should  be  the  same  as  before. 

All  the  required  initial  values  have  now  been  calculated,  and  the  resulting  output 
is  [Ref  14] 


P  0 


wb0 

q 


(2.C.16) 


2.  The  Flight  Program 

Once  the  previous  iteration  (or  the  initialization)  has  been  integrated,  it  is  returned 


to  the  program  as  the  current  values.  The  program  uses  these  values  to  calculate  all  the 
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descriptive  values  of  the  system,  apply  the  corrective  time-  and  position-  dependant 
factors,  and  calculate  the  derivatives  for  the  next  iteration. 


The  angle  of  attack  is  tan 


-1 


f ..  \ 


vvA-y 


and  the  sideslip  angle  is  tan 


-1 


vv*y 


The 


program  then  calculates  the  necessary  control  surface  deflections  to  zero  the  angle  of 
attack  and  the  sideslip  angle,  to  maintain  the  alignment  of  the  missile  axis  and  its  velocity 
vector. 


The  geocentric  latitude  is  derived  from  the  Cartesian  coordinates  of  the  position 


[p] 


latgc  =  tan 


-1 


2  2 

P*  +Py  j 


(2.C.17) 


The  celestial  longitude  is  derived  from  rectangular  coordinates  of  [p]  with  time- 
dependant  correction  factors,  subject  to  a  principle  value  requirement  (between  -180  and 
+  180) 


=  tan 


p  \ 

—  +  ILong  +  WzEt 

[Pj 


(2.C.18) 


Determining  the  geodetic  coordinates  requires  an  iterative  process  because  the 
prime  radius  of  curvature,  N,  is  a  function  of  the  geodetic  latitude,  (p,  [Ref  14] 
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(2.C.19) 


h  =  h  +  Ah 

The  ranges  and  required  accuracy  of  this  model  prohibit  the  assumption  of  a  flat 
earth  with  a  constant  gravitational  acceleration  vector.  The  gravitational  forces  on  the 
missile  are  a  position  dependant  correction  factor. 

[l  +  1.5J2(Re/  r)2  (1-5  sin2  y/)]px  /  r 
[1  +  1.5  J2 (Re/  rf  (1-5  sin2  y/)\pv  /  r  (2.C.20) 

[1  + 1.5  J2 (Re/  r)2  (3 -5  sin2  y/)\P-  /  r 

The  atmospheric  affects  on  the  missile  are  dependant  on  the  altitude,  which  is 
derived  using  the  STatmos.m  function  described  in  section  D. 

The  missiles  speed  in  m/s  is  determined  by  the  normalization  of  the  velocity 
vector,  vb.  The  missiles  Mach  number  is  determined  by  dividing  the  speed  by  the  local 
Mach  number  which  is  determined  by  the  local  temperature  from  the  STatmos.m 

program  ( M  =  yj/RT  ). 

The  moment  matrix  and  missile  mass  is  detennined  as  a  function  of  time  and  is 
based  on  assumed  fuel  usage  parameters.  The  missile  program  calls  a  separate  function, 
either  SMParams.m,  to  evaluate  the  specific  descriptive  parameters  of  the  ballistic 
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missile.  The  program  assumes  a  cruciform  missile  in  two  stages  plus  a  conical  nosecone 
section,  where  only  the  first  stage  separates  after  completion. 

The  dimensions  of  the  remaining  fuel  are  critical  to  the  purposes  of  calculating 
the  center  of  gravity.  For  the  interceptor,  the  first  stage  is  active  for  6  s  while  the  stage  1 
fuel  is  consumed.  The  fuel  is  solid,  so  the  only  parameters  to  change  are  the  length  of  the 
fuel  and  the  inner  radius  as  it  is  consumed.  Once  the  fuel  is  totally  consumed  in  stage  1, 
the  booster  separates  and  stage  2  takes  over  in  a  similar  manner.  Stage  2  lasts  for  an 
additional  20  s.  For  the  rocket,  the  first  stage  is  active  for  125  s  while  the  stage  1  fuel  is 
consumed.  The  canister  the  fuel  is  contained  in  is  assumed  to  be  a  constant  size  and 
diameter,  so  the  only  parameter  to  change  is  the  length  of  the  fuel  as  it  is  consumed.  The 
missile  is  in  a  constant  forward  acceleration,  so  the  fuel  is  assumed  to  remain  in  the  rear 
of  the  canister  for  the  purposes  of  calculating  its  center  of  gravity.  Thus  the  center  of 
gravity  of  the  fuel  tends  toward  the  rear  of  the  missile  through  the  flight.  Once  the  fuel  is 
totally  consumed  in  stage  1,  the  booster  separates  and  stage  2  takes  over  in  a  similar 
manner.  Stage  2  lasts  for  110  seconds. 

The  center  of  gravity  (CG)  is  determined  by  the  parallel  axis  theorem  applied 
independently  to  the  x,  y,  and  z  axes.  The  missile  was  separated  into  five  separate  pieces: 
nosecone,  stage  1  structure,  stage  2  structure,  stage  1  fuel,  and  stage  2  fuel.  Each  CG  was 
separately  calculated  and  then  combined.  The  CG  of  the  individual  structural 
components  was  a  simple  matter  of  their  distance  from  the  nosecone.  The  center  of 
gravity  of  the  fuel  was  then  based  on  the  fuel  remaining  in  the  canister,  referenced  to  the 
base  of  the  missile  section.  The  missile  CG  is  calculated  from  the  locations  and  masses 
of  the  component  sections: 


^  missile  CG missile  ^  nose  CGnose 


+  M 


st 2  str 


+  M stl_strCGsti_str  +  M  sti_fUelCGstljUel 

CGst  2_str  +  M st2_fiielGGst2_fueI 


(2.C.21) 


The  moments  of  inertia  for  the  x,  y,  and  z-axes  are  calculated  using  the  CG  and 
the  remaining  fuel  length  using  a  similar  methodology  of  components.  For  the  rocket  the 
equations  are 
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Jxx 

Jyy  Jzz 

Nosecone 

—  M  r 2 

^  q  nose 

3  ( r2  ^  (  5  V 

~Mnose  -  +  h 2  +  Mnose  CG  -  —Lnose 

0  f  4  J  V  0  J 
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Structure 

[>„2  +  <T)  +  Lj]+Ms:r(CG  -  24f 
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Fuel 

Mfuelr 

2 

^|i(3r2  +  Lj )  +  Mf,JCG  -  (32 -Lf,„)) 

Stage  2 

Structure 

[3(r„2  + 1-2)  +  Lj]  +  MJCG- 9)2 
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Fuel 
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2 

O2  +  Lj )  +  Mmi(CG  -  (1 6  -  Lfu„ )) 

The  equations  for  the  interceptor  are  similar. 

The  J  matrix  returned  is  the  symmetrical  matrix,  since  the  ballistic  missile  itself  is 
symmetric: 


J  = 


0 

0 


0 

J 

y: 

o 


(2.C.23) 


The  BRParams.m  and  SMParams.m  functions  also  determine  the  reference  areas 
for  the  control  surfaces  and  missile  planfonn  areas  using  simple  equations  from  Zarchan 
[Ref  21].  The  function  returns  the  base  diameter  (dia),  reference  area  (Srej),  planfonn 
area  ( Spian ),  wing  area  (Swing),  tail  area  (Stau),  nose  area  (An),  body  area  (Ab),  nose  center  of 
pressure  (Xcpn),  and  body  center  of  pressure  (Xcpb)  according  to 
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Sref  = 
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(2.C.24) 


The  program  then  calls  the  function  ACoeff.m  to  determine  several  necessary 
aerodynamic  coefficients.  The  function  inputs  are  angle  of  attack,  altitude,  and  pitch 
control  surface  deflection.  The  pitch  control  surface  is  specifically  included  because  it 
alone  varies  over  the  spectrum  of  possible  angles,  from  -20°  to  +20°,  whereas  the  roll  and 
yaw  derivatives  can  be  assumed  to  be  constant  over  that  range  of  possible  angles.  The 
pitch  deflections  therefore  require  a  second  interpolation  to  determine  their  actual  value 
at  each  time  step. 

Figures  14-20  detail  the  range  of  values  of  the  variables  returned  after 
interpolation  (all  figures  after  [Ref  7]).  Some  variables  require  multiple  interpolations, 
such  as  in  Figures  14  and  19. 


Stability  Data,  Pitch  Plane 


Figure  14.  Pitch  plane  stability  data 
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Yaw  Stability  Derivatives 


Figure  15.  Yaw  stability  and  control  derivatives 
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Figure  16.  Roll  control  derivatives 
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-0.04 


Roll  Control  Coupling  Derivatives 


0  5  10  15  20 

Angle  of  Attack  (deg) 

Figure  17.  Roll  control  Coupling  Derivatives 
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Figure  18.  Yaw  control  Derivatives 
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Effect  of  Pitch  Control  on  Roll  Stability 


Figure  19.  Pitch  control  effects  on  roll  stability 


Pitch  Control  Derivatives/Aeroelastic  Missile 
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Figure  20.  Pitch  control  derivatives 


The  returned  values  are  combined  together  to  calculate  the  force  and  moment 
coefficients  [Ref  7] 
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(2.C.25) 


CY(a,j3,SP, SY, SR)  =  CY  ft  +  CYgi SY  +  CYgg SR 
CN{a,SP)  =  CNa  +  CNJP 
C,(a,j8,SP,  SY,  SR)  =  ClfP  +  Clgr  SY  +  C!<;r  SR 
Cm(a,SP)  =  Cma  +  CmJP 
C(a,j3,SP,SY,SR)  =  Cn  fi  +  Cn  SY  +  Cn  SR 

The  program  calls  the  function  ZLDragC.m  to  detennine  the  value  of  the  drag 
coefficient.  The  Thrust  is  determined  exactly  as  before,  by  simple  time  dependence.  The 
total  force  on  the  body  of  the  missile  from  all  lift,  drag,  thrust,  and  aerodynamic  forces  is 
then  [Ref  22] 

fx  1  sinal  ^Thrust]  Wx~f 

[/]*=/,=  +  0  V,  (2.C.26) 

_/J  _q roSrefCN  cos  a  _  L  0  J  [k_ 

The  torque  experienced  by  the  missile  from  all  moment  forces  is  [Ref  7] 

~C,~ 

T  =  qroSrefd  cm  (2.C.27) 

C„_ 

The  inertial-body  centered  rotation  matrix  is  determined  using  the  quaternion 

(tfo  +  tfi  - - ^3 )  +  QoQi)  2(gjg3  -q0q2) 

Kn  =  2(t/y/2  -t/(y/3)  (ql  -  qt  +  q\  -  q])  2( t/2ty,  +  ty0ty, )  (2.C.28) 

2(qlq3+q0q2)  2{q2q3  -  q0qx)  (ql  -  q\  -  q\  +  q\) _ 

Also,  as  before,  the  ECI-inertial  rotation  matrix  is  determined  as 

cos  {ILong  +  WzEt )  sin  {ILong  +  WzEt)  0 

i?e/i  =  -  sin(/To«g  +  cos  (ILong +  WzEt)  0  (2.C.29) 

0  0  1 

The  total  rotation  matrix  is  then 


^6/n  —  C^n/ 


(2.C.30) 


The  Euler  angles  with  respect  to  the  ECEF  system  are  [Ref  8] 
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(f)  =  tan 


-1 


^/,(2,3) 

V  ^bin  (3,  3)  j 


y/  =  tan 


-1 

-1 


9  =  -  sin-1  (i?fc/n(l,3)) 


(2.C.31) 


V 


The  quaternion  rotation  matrix  is  calculated  is  [Ref  14] 

0  -P  -Q  -R 
P  0  R  -Q 
Q  -R  0  P 
R  Q  -P  0 


Q  = 


(2.C.32) 


The  centripetal  acceleration  due  to  the  rotation  of  the  missile  and  its  velocity  is 
the  vector  product  [Ref  14] 


^b/i  - 


o  -R  Q 
R  0  P 
-Q  P  0 


(2.C.33) 


The  centripetal  acceleration  due  to  the  Earth's  angular  velocity  and  the  rotation  in 
the  ECI  frame  is  the  vector  triple  product  [Ref  14] 


O'  = 

“ e!i 


0 

0 


-WzE  0 
0  0 
0  0 


(2.C.34) 


All  values,  coefficient,  and  matrices  have  been  determined.  The  full  state 
parameters  can  be  calculated  as  [Ref  14] 

ipicM/o=Rb/i*vb+Cieii*Pi 


b  • b 


1 


VcM/e  =  ■ -Kt + Rbn  *  (O'  -  ae/i  *  p'CMIO)  -  (n>blt + Rb/ine/iRTb/i)  *  vf 


m 

b  jJ)  /  rb\- 1 


CM  I  e 


(2.C.35) 


1 ib/i  —  ~  9 b/i  *^b/i 


38 


3.  Results 

In  order  to  fully  visualize  the  geometry  of  the  intercept,  an  animation  function 
was  developed  that  shows  side-by-side  the  orientation  of  the  target  and  the  interceptor.  A 
screen  shot  of  the  plot  is  shown  in  Figure  21.  The  animation  shows  the  stage  changes  of 
the  missile  as  well,  dropping  the  used  stages  at  the  appropriate  moment. 
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Figure  2 1 .  6DOF  Orientation  Animation 


D.  COMMON  FUNCTIONS 

Two  functions  were  common  to  all  the  models,  ZLDragC.m  and  STatmos.m. 

1.  ZLDragC.m 

The  drag  on  the  missile  is  dependant  on  two  conditions,  the  Mach  number  and 
whether  the  missile  is  in  the  boost  or  glide  phase.  The  phase  of  the  missile  is  easily 
determinable  from  the  time. 

The  reason  the  values  differ  is  the  presence  or  absence  of  the  base  drag.  When 

the  rocket  is  under  power,  the  thrust  pressure  is  balanced  to  atmospheric,  so  there  are  no 

parasitic  drag  effects  from  the  tail  section.  After  the  engine  shuts  down,  there  is  no 

longer  any  pressure  generated,  so  the  sharp  end  of  the  tail  section  generates  a  drag  effect. 
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Parasitic  drag  from  the  rest  of  the  missile  is  relatively  constant  in  both  phases  of  the 
missile  flight. 

There  is  a  sharp  increase  in  the  drag  effects  at  Mach  one,  after  which  the  drag 
drops  considerably  [Ref  8].  Figure  22  shows  the  effect  of  speed  on  the  drag  coefficient. 


Drag  Coefficient  by  Mach  Number  and  Boost  Phase 


Mach  Number 


Figure  22.  Drag  Coefficient  by  Mach  Number  and  Flight  Phase 


The  drag  force  is  then  calculated  from 


Drag  -  CL 


pV 2 


(2.D.1) 


and  the  atmospheric  dynamic  pressure  is  calculated  from 

pV 2 

< Iro  = 


(2.D.2) 


2.  STatmos.m 

Many  of  the  atmospheric  affects  on  the  missile  are  dependant  on  the  altitude.  The 
missile  program  calls  a  separate  script,  STAtmos.m,  to  determine  the  density,  pressure, 
and  temperature  of  the  local  atmosphere.  The  script  is  based  on  the  1976  standard 
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atmospheric  survey,  and  includes  values  up  to  86  km  in  a  tabular  format.  Figures  23-25 
show  the  details  of  the  values  returned. 


Atmospheric  Temperature  Variation  by  Altitude 


Temperature  (K) 

Figure  23.  Atmospheric  Temperature  Variation  by  Altitude 
Atmospheric  Density  Variation  by  Altitude 


Figure  24.  Atmospheric  Density  by  Altitude 
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Atmospheric  Pressure  Variation  by  Altitude 


Figure  25.  Atmospheric  Pressure  Variation  by  Altitude 
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III.  EXISTING  GUIDANCE  LAWS 


There  are  a  wide  variety  of  guidance  laws  available  for  missile  guidance  processing. 
Many  of  them  are  not  acceptable  for  this  application  such  as  Beam  Rider  and  Pure  Pursuit,  while 
others  are  acceptable  though  not  optimal  such  as  Proportional  Navigation  and  its  variants. 


A.  UNACCEPTABLE  GUIDANCE  LAWS 

1.  Beam  Rider 

Beam  Rider  (also  called  Command  Line-of-Sight)  is  among  the  simplest  fonns  of 
command  guidance.  The  missile  simply  flies  along  a  tracker  beam  that  is  continuously  pointed  at 
the  target.  In  other  words,  the  inertial  velocity  vector  is  continuously  pointed  at  the  target.  The 
beam  and  the  missile  generally  originate  from  the  same  position,  but  not  always.  The  guidance 
commands  are  proportional  to  the  angular  error  between  the  missile  and  the  beam,  and  if  the 
missile  remains  exactly  on  the  beam  it  will  hit  the  target.  There  is  no  consideration  for  the 
capabilities  of  the  target,  and  the  missile  is  not  directed  to  orient  itself  to  lead  the  target  in  order 
to  anticipate  its  movements.  The  missile  therefore  requires  much  greater  maneuvering 
capabilities  in  the  moments  just  prior  to  interception.  This  fonn  of  guidance  law  also  assumes 
that  the  shooter  will  have  a  direct  line  of  sight  for  the  entire  engagement,  and  that  the  diffraction 
of  the  beam  will  be  negligible,  which  is  obviously  not  true  over  the  distances  required  for  a  BPI. 
[Ref  2] 


2.  Pure  Pursuit 

Pure  Pursuit  overcomes  this  limitation  by  having  the  missile  supply  the  targeting  data 
instead  of  a  third-party  observer.  This  is,  in  effect,  Beam  Rider  Guidance  where  the  beam  is 
generated  onboard  the  missile.  As  before,  the  missile  only  follows  the  beam  and  the  guidance 
commands  are  proportional  to  the  angular  error  between  the  missile  and  the  line  of  sight  to  the 
target.  This  overcomes  the  diffraction  of  the  beam,  but  still  does  not  command  the  missile  to 
orient  itself  to  lead  the  target.  It  thus  requires  similarly  large  maneuvering  capabilities  to 
complete  the  interception. 

Both  of  these  guidance  laws  are  severely  limited  in  their  effectiveness,  are  used 
only  at  short  range  against  non-maneuvering  or  relatively  slow  targets,  and  require 


significant  interceptor  capabilities  relative  to  the  target.  This  is  not  an  accurate 
description  of  the  requirements  for  a  BPI,  and  both  laws  are  therefore  discounted  for  use 
here.  [Ref  2] 

B.  LESS  THAN  OPTIMAL  GUIDANCE  LAWS 

1.  True  Proportional  Navigation 

Another  family  of  guidance  laws  involves  Proportional  Navigation  (PN)  and  its 
derivatives.  Of  the  three  basic  guidance  laws,  proportional  navigation  is  the  most  versatile,  and 
therefore  most  frequently  implemented,  making  it  the  guidance  law  of  choice  in  nearly  all 
modem  guided  missiles.  In  proportional  navigation  the  rate  of  change  of  the  missile  heading  is 
proportional  to  the  rate  of  rotation  of  the  line-of-sight  (LOS)  from  the  missile  to  the  target.  The 
guidance  system  points  the  differential  velocity  vector  at  the  target  [Ref  2] 

In  order  for  interception  to  occur,  the  heading  angle  and  the  LOS  angle,  X,  must  remain 
constant.  If  the  target  increases  speed,  then  X  will  increase  and  the  interceptor  must  increase  its 
heading  *P  to  compensate.  Thus  it  is  apparent  that  they  must  be  proportional  to  each  other,  or 
more  accurately,  the  rate  of  change  of  each  must  be  proportional,  [Ref  21] 

W  =  NX  (3.B.1) 

where  N  is  the  proportionality  constant,  which  detennines  the  amount  of  target  lead  by  the 
interceptor  (usually  3-5  depending  on  the  interceptor’s  maneuvering  capabilities).  The 
interceptor  maneuvers  until  2  =  0  ,  at  which  point  4*  =  0  and  no  further  maneuvers  take  place 
until  intercept. 

The  missile  seeker  measures  the  LOS  rate  and  the  PN  guidance  law  converts  that  into  an 
acceleration  command  by  the  guidance  computer.  Using  the  fact  that  the  acceleration  nonnal  to 
the  velocity  vector  of  the  interceptor  is 

a  =  VDx ¥  (3.B.2) 

which  can  be  correlated  to  the  LOS  rate  to  develop  the  steering  command  for  the  guidance 
computer 

a  =  NVdX  (3.B.3) 

The  same  method  applies  to  the  extension  of  this  law  into  three  dimensions. 


Using  angular  velocity  vectors  between  the  target  and  interceptor,  Zipfel  [Ref  22] 
develops  a  three  dimensional  PN  guidance  law  as 

a  =  NVdQoeuv  -  g  (3.B.4) 

where  Qri  is  the  cross  product  of  the  LOS  frame  with  the  Earth  and  the  velocity  vector  with 
respect  to  the  Earth,  uv  is  the  unit  vector  of  Vr> ,  and  g  is  the  added  gravity  bias. 

2.  Compensated  Proportional  Navigation 

True  PN  is  rarely  implemented  in  this  simple  fonn,  however.  The  thrust  generated  by 
the  interceptors  motor  creates  a  parasitic  acceleration  in  the  LOS  angle  that  must  be 
compensated  for  in  the  autopilot  command  or  intercept  errors  will  occur.  Such  a  guidance  law  is 
tenned  “compensated”.  The  missile  acceleration  is  projected  onto  the  LOS  plane  by 

wr-^Rwr  (3-b-5) 

and  subtracted  from  the  PN  command  in  equation  (3.B.4)  to  obtain  the  augmented  command 
[Ref 22] 

a  =  NV°  [n“]'  [uj  -  “*WR  [a.]"  -  [g]"  (3.B.6) 

where  the  rotation  matrix  L0^R  of  the  LOS  coordinates  with  respect  to  the  wind  frame  is  defined 
by  the  azimuth  and  elevation  angles  from  the  LOS  vector. 

3.  Augmented  Proportional  Navigation 

Zarchan  details  one  further  variation  of  PN,  tenned  Augmented  Proportional  Navigation 
which  includes  an  extra  tenn  to  account  for  the  acceleration  of  the  target  [Ref  21]. 

a  =  NVDCiOEuv  +-^~nT-  g  (3.B.7) 

where  nT  is  the  acceleration  of  the  target.  However,  a  detailed  knowledge  of  the  target’s 
acceleration  requires  an  advanced  Kalman  filter,  such  as  an  Extended  Kalman  Filter  or  an 
Alpha-Beta-Gamma  Filter,  which  are  not  sufficiently  accurate  [Ref  13]  and  is  not  feasible 
to  be  implemented  onboard  an  interceptor  as  small  as  the  Standard  Missile.  Additionally, 
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application  of  the  acceleration  would  require  exact  knowledge  of  the  target’s  staging 
events  where  the  acceleration  value  changes  radically. 


C.  KEY  FATAL  CHARACTERISTICS 

There  are  three  fatal  characteristics  of  these  guidance  laws  which  can  preclude  the 
interceptor  from  impacting  the  target:  the  dynamics  of  the  missile  controls  system,  the 
lack  of  knowledge  of  the  end-game  environment,  and  the  inability  to  control  the  intercept 
geometry. 


1.  Control  System  Time  Constant 

If  the  dynamics  of  the  seeker,  noise  filter  and  flight  control  system  are  neglected, 
a  perfect  or  zero-lag  guidance  system  is  achieved  and  the  missile  will  always  hit  the 
target  if  it  is  tracked  correctly.  In  reality,  guidance  commands  can  not  be  implemented 
instantaneously  and  there  will  be  lags  or  dynamics  within  the  guidance  system.  In 
missile  systems,  the  dominant  portion  of  the  total  system  time  constant  is  usually 
associated  with  the  flight  control  system  actuators  such  as  the  tail  fin  control  surfaces 
[Ref  21],  Such  time  lags  are  generally  represented  by  a  time  constant 


1 


1  +  sT 


(3.B.8) 


where  n,  is  the  achieved  missile  acceleration,  nc  is  the  commanded  missile  acceleration, 

and  T  is  the  flight  control  system  time  constant.  Zarchan  has  shown  that  even  a  very 
good  1 -second  time  constant  can  have  a  considerable  impact  on  the  miss  distance  [Ref 
21],  A  system  with  a  small  guidance  system  time  constant  has  the  potential  for  having 
very  small  miss  distances.  However,  technology  issues  limit  how  small  the  guidance 
system  time  constant  can  be.  Thus  all  systems  that  utilize  current  target  information  in  a 
homing  guidance  loop  with  a  feedback  control  system  will  have  some  miss  distance  due 
to  the  flight  control  system  time  constant. 
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2.  End-game  Environment 

The  lack  of  knowledge  of  end-game  environment  is  also  a  major  problem. 
Missiles  use  fuel  for  a  fraction  of  the  flight  to  generate  a  set  amount  of  thrust  and  speed, 
after  which  the  missile  must  glide  to  the  target  under  a  constant  drag-  and  gravity-induced 
deceleration.  The  amount  of  available  maneuver  capability  depends  on  the  missile  speed 
and  altitude  of  engagement  -  higher  speeds  and  lower  engagement  altitudes  work  to 
increase  the  missile  capability  [Ref  20],  Therefore  trajectories  should  be  flown  to 
maximize  the  missile  velocity  and  minimize  the  intercept  altitude  so  that  there  is 
sufficient  acceleration  left  to  intercept  the  target. 

Yet  the  above  guidance  laws  do  not  consider  the  end-game  prior  to  achieving  it. 
The  effect  is  to  guide  the  missile  along  the  most  direct  path  and  hope  that  the  interception 
will  take  place  under  atmospheric  conditions  that  are  acceptable  to  the  interceptor  and  to 
try  to  minimize  the  acceleration  at  each  moment  with  the  hope  that  there  will  be  enough 
capability  remaining  to  complete  the  end-game.  The  assumption  that  target  maneuvers 
will  be  sufficiently  small  enough  that  the  missile  will  have  enough  capability  to  counter 
them  is  usually  accurate;  however,  the  immense  accelerations  involved  in  a  BPI  makes 
active  management  of  the  missile’s  acceleration  critical  to  ensuring  the  end-game  is 
successful. 


3.  Controlling  the  Interception  Geometry 

The  interception  geometry  of  any  engagement  under  the  above  guidance  laws  is  a 
result  of  the  relative  speeds  of  the  missile  and  target,  the  launch  geometry,  and  the 
maneuvers  involved.  The  above  guidance  laws  have  no  control  over  any  of  these  aspects, 
though  this  geometry  is  critical  to  ensuring  the  interceptor  is  capable  of  destroying  the 
ICBM.  An  overtaking  intercept  (the  interceptor  approaches  the  target  from  the  rear)  is 
significantly  less  desirable  than  a  head-on  or  a  right-angle  intercept. 

Only  if  the  intercept  geometry  is  controllable  can  it  be  expected  to  conform  to  a 
pre-detennined  setup.  Since  none  of  these  guidance  laws  can  control  the  geometry,  they 
are  not  capable  of  maximizing  the  kinetic  energy  transfer  via  the  intercept  geometry. 
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IV.  PROPOSED  ALGORITHM 


A  method  that  overcomes  fatal  characteristics  of  modem  guidance  laws  is  the  key 
driving  factor  in  the  development  of  this  advanced  guidance  law.  The  guidance  law  will 
determine  the  near-optimal  flight  path  from  the  interceptor  position  to  a  predicted  target 
position  for  the  interceptor  to  follow  to  intercept,  and  then  derive  the  set  of  control 
commands  necessary  to  execute  that  flight.  This  section  will  describe  a  method  for 
deriving  that  trajectory  using  calculus  of  variations  based  on  three  cues:  high-order 
polynomials  as  a  reference  function  for  the  flight  path,  a  preset  thrust  history  as  one  of 
the  controls,  and  a  few  optimization  parameters.  The  trajectory  optimization  problem  is 
then  converted  into  a  nonlinear  programming  problem  and  solved  numerically. 


A.  PROBLEM  STATEMENT 

Among  all  admissible  trajectories, 

z(t)  =  {zl(t),z2(t),...,zr(t)}r  eS 

(  >  i-  i  (4.A.1) 

S  =  | z(t)  e  Z'  c=  E’  j,  t  e  [t0^/] 

that  satisfies: 

1.  The  system  of  differential  equations  (dynamic  constraints): 

Zi  =  fi(t,  Z,  U,  a),  i  =  1,  r  (4.A.2) 

where  the  vector  of  controls  is 

u(t)  =  {ul(t),u2(t),...,um(t)}T ,  m<r,  u  eUm  a  E’"  and  the  vector  of 
missile  parameters  is  a  =  (a1,a2,...,ap),  a  e  Ap  cz  Ep ; 

2.  The  initial  conditions: 

z(t0)eS0,  S0  -  {z0  eZ'cf'}  (4.A.3) 

u(t0)  e  R0,  R0  =  jw0  e  Um  a  E'"  j  (4 .A. 4) 

and  the  final  conditions: 
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2{tf)eSf,  Sf  =  ^zf  e  Z'  c  Er;  Gj  (z  (f,  ))  =  0,  j  =  1,  /}  (4.A.5) 

«(fr)  e  i?/5  i?7  =  {i^  g  Um  c=  £m}  (4.A.6) 

3.  The  constraints  imposed  on  the  state  space 

jj(t,z)  =  {jil(t,z),jj2(t,z),...,TiM(t,z)}T  >0  (4.A.7) 

on  the  controls 

g(t,z,u)  =  {gl(t,z,u),g2(t,z,u),...,gv(t,z,u)}T  >0  (4.A.8) 

and  on  the  controls  derivatives 

n{t,u,u)  =  ^7rx{t,u,u),7r2{t,u,u),...,7r0{t,u,u)^  >0  (4.A.9) 


Find  the  optimal  trajectory,  z  t(t) ,  that  minimizes  the  integral  function 

*f 

J  =  K(x0,xf)  +  J  L(t,z,u)dt  (4.A.10) 

*0 

and  the  corresponding  optimal  controls,  uopt{t) ,  where  K,  L  are  defined  functions. 

B.  CALCULUS  OF  VARIATIONS 

Calculus  of  variations  deals  with  functions  of  functions,  termed  functionals, 
instead  of  functions  of  some  variable  or  variables  as  in  ordinary  calculus.  Specific 
interest  is  in  the  extremals  of  these  functionals  -  those  making  the  functional  attain  a 
maximum  or  minimum  value  [Ref  15].  There  are  two  broad  categorizations  of  methods 
to  solve  these  problems,  indirect  methods  and  direct  methods. 

Indirect  methods  resolve  the  problem  into  a  differential  equation,  usually  via  the 
difference  of  a  series  of  equations  of  motion  and  thus  solve  the  general  theory  of  partial 
differential  equations.  This  method  does  not  assume  anything  about  the  solution,  leading 
to  a  very  complete  and  perfectly  precise  answer;  however,  one  must  integrate  the 
resultant  equation  to  derive  the  extremals.  The  precision  greatly  increases  the 
computational  complexity,  and  therefore  the  time  required  to  solve,  for  negligible  gain  in 
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optimality.  Further,  these  differential  equations  are  difficult  to  integrate  except  in  the 
simplest  of  cases,  and  nearly  impossible  to  program  [Ref  19].  This  approach  is  further 
complicated  by  the  need  to  solve  the  problem  in  a  specified  fixed  region  instead  of  in  the 
small  neighborhood  of  some  point.  These  difficulties  can  be  overcome  by  using  direct 
methods,  which  do  not  reduce  the  variational  problems  to  ones  involving  differential 
equations. 

The  fundamental  idea  of  direct  methods  is  to  consider  a  variational  problem  as  a 
limit  problem  of  the  extreme  of  a  function  of  a  finite  number  of  variables  that  can  be 
solved  by  numerical  methods.  Two  basic  direct  methods  are  the  Rayleigh-Ritz  and 
Galerkin  methods,  which  assume  the  solution  to  be  an  unknown  function  but  of  a  certain 
form  containing  a  set  of  unknown  coefficients  (themselves  functions  of  the  boundary 
conditions),  which  are  then  found  by  minimization.  The  practical  result  is  that  the 
problem  has  been  reduced  from  calculus  to  algebra,  but  at  the  cost  of  a  significant 
increase  in  the  number  of  simultaneous  equations  to  be  solved.  It  is  for  this  reason  that 
little  work  was  done  in  this  field  until  the  advent  of  computer  technology.  The  possible 
solution  set  is  restricted  to  a  smaller  space  than  the  original  equation  because  of  the  initial 
assumption  regarding  the  solution,  but  the  resultant  problem  can  be  programmed  and 
quickly  solved  using  computers;  however,  the  solutions  are  only  an  approximation  of  the 
original  solution.  Therefore  these  solutions  can  only  be  regarded  as  near-optimal 
solutions. 

Professor  Taranenko  [Ref  19]  first  applied  the  ideas  of  direct  methods  and  the 
combination  of  Ritz  and  Galerkin  methods  to  the  problems  of  flight  dynamics  by 
identifying  a  reference  function  for  the  flight  vehicle’s  motion  and  velocity 

xt  =  xi0  +  ( xif  - xi0 )  T  r°  +  O . (r),  /  =  1,4  (2.1) 

Tf~T0 

where  x1,x2,x3,are  the  Cartesian  coordinates  for  the  flight  path,  x4  is  the  velocity,  and 
®,(r)  is  a  continuous,  unequivocal,  differentiable  function  satisfying  the  boundary 
conditions  ®;(r0)  =  dfG, )  =  0  .  Any  function  that  satisfies  those  conditions  would  be 
acceptable  for  use.  Taranenko  called  r  a  virtual  arc.  It  is  this  critical  variable  that  allows 
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the  separation  of  the  spatial  trajectory  from  the  velocity  and  thus  optimizes  one  or  the 
other,  or  both  independently.  The  specific  task  determines  the  appropriate  choice  of  x  , 
but  in  general  any  continuous,  monatomic  function  is  acceptable:  time,  path,  energy,  etc. 
The  remaining  state  parameters  and  flight  controls  are  then  determined  by  solving  the 
inverse  problem  of  flight  dynamics.  Rather  than  starting  with  the  control  time  histories 
and  integrating  them  to  detennine  the  flight  path,  this  method  starts  with  a  flight  path  and 
determines  the  control  time  histories  necessary  to  create  it. 

In  order  to  implement  a  solution  to  this  problem  that  can  be  solved  in  real  time,  a 
further  restriction  must  be  made.  The  continuous  problem  must  be  discretized  to  reduce 
the  infinite  variational  problem  to  one  of  optimization  of  few  parameters  at  numerous 
sampling  points.  This  allows  for  the  optimization  of  the  planar  trajectory  of  the  missile  at 
several  points  along  the  path  by  presetting  the  state  variables  and  one  of  the  controls’ 
time  histories,  and  then  solving  the  inverse  flight  dynamics  problem. 

These  methods  have  all  previously  been  used  for  off-line  optimization  of 
trajectories,  but  none  has  yet  been  applied  to  the  real-time  onboard  optimization  of  a 
missile  flight  path.  Yakimenko  detailed  a  method  called  a  Direct  Method  for  Rapid 
Prototyping,  which  he  applied  to  short  tenn  spatial  trajectories  of  aircraft  maneuvers 
using  fixed  boundary  points  [Ref  19].  The  developed  program  presented  here  uses  similar 
numerical  method  to  provide  a  near-optimal  spatial  trajectory  that  is  completely  defined 
by  a  few  optimization  parameters,  but  as  will  be  discussed  shortly,  has  fluid  final 
boundary  conditions. 

Though  the  method  artificially  limits  the  possible  trajectory  variations,  it  does 
guarantee  the  following  [Ref  19]: 

1 .  The  boundary  conditions  are  satisfied  a  priori, 

2.  The  control  commands  are  physically  realizable  and  smooth, 

3.  Only  a  few  variable  parameters  are  used,  thus  ensuring  that  the  iterative 
process  converges  well, 

4.  The  near-optimal  solution  is  very  close  to  the  optimal  one. 
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A  simple  two  dimensional  variation  program  of  a  similar  7th  order  system  will 
demonstrate  how  the  direct  method  varies  the  flight  path  according  to  the  boundary 
conditions.  The  boundary  conditions  are 
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where  the  third  derivative  of  the  initial  x,  condition  has  been  set  to  vary  according  to 

x'"10  =  {-0.4; -0.1;  0.2;  0.5} 
and  the  length  of  the  virtual  arc  varies  according  to 

Tf  =1,2,. ..,10 

The  resulting  set  of  paths  is  shown  in  Figure  26  and  the  first  derivative  of  the  path  is 
shown  in  Figure  27.  It  is  important  to  note  that  the  first  derivative  is  not  “velocity”  but  is 
instead  the  “rate  of  change  of  the  path”.  It  is  proportional  but  not  equal  to  velocity, 
specifically  because  of  the  virtual  variable  r  as  discussed  previously.  Each  line 
represents  a  different  choice  of  z, ,  showing  that  by  varying  that  value  the  length  of  the 

path  can  change  drastically.  The  algorithm  developed  here  will  use  this  technique  to 
derive  the  optimal  flight  path. 
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Figure  26.  Variation  of  Path  with  z  f  and  x”  (after  [Ref  18]) 
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Figure  27.  Variation  of  First  Derivative  of  Path  with  z  f  and  x'"0 
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C.  PROGRAM  DEVELOPMENT 

1.  Boundary  Conditions 

Using  equations  (3.2)  and  (3.3),  in  addition  to  the  controls  nx,nr,n_,  it  is  possible 
to  construct  the  vector  of  state  variables  z  =  {xl,x2,x3,V,0,x¥}T  and  the  vector  of  controls 
u  =  {nx,n  ,n,}T .  The  model  for  thrust,  drag,  and  missile  characteristics  is  the  same  as 
previously  used  in  Chapter  3. 

The  beginning  assumption  is  that  the  following  data  is  known  by  the  interceptor’s 
onboard  computer: 


Interceptor 

Target 

Body  Frame 

*iOo)  =  *io 

*2  (t0  )  —  *20 

X3  (^0  )  —  *30 

V(to)  —  Vq  *10 

Wo)  —  ^0  '>"  *20  *10 

0(tO )  —  ^0  _ .  *30  *20 

ny(t0)  =  ny0  *30 

n:(t0)  =  nz  o 

*i0/)  =  *i/ 

*2(t/)  =  *2/ 

*3(t/)  =  *3/ 

V(tf)  =  Vf  xlf 

y(tf)=x vf  k/ 

O(tf)  =  0f  J  x3/ 

Earth  Centered 
Inertial 

*r«,  *2s M(o,  *3w(o 

*fM(0,  x2M(0,  *f(0 
xT(t),  *f(0,  *f  (0 

*r«,  *f  *2m(o 
*r«,  *25s(o,  *fco 

Table  7.  Interceptor  Known  Data 


The  target  data  will  be  used  to  determine  the  final  boundary  conditions  of  the 
interceptor  missile  according  to  the  time  until  intercept,  At : 

Position: 
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XSM  =xBR  +xBR  At 

A1  f  A10  T  A10 

x™  =x™+x™  At  (4.C.1) 

x™=x™+x°«At 

Heading  and  Flight  Path  Angle: 

qSM  _  +  qBR  wJ-jCrc  qBR  _  arctg  *3  = 

’  2  ’  '  Jx‘‘2+x“ 2 

V  1  2  (4.C.2) 

•  BR 

wT  =  y/HfR  > where  <=arctgi 

•fi 

which,  when  combined  with  reasonable  estimates  of  the  final  values  of  the  velocity,  V, 
and  the  time  rate  of  change  of  velocity,  V ,  heading  ( 4*  =  0  ),  and  flight  path  angle 
(0  =  0),  yields  the  final  conditions: 

xfj  =  Vf  cos  0f  cos  y/f 

*2/  =Vf  cos0fsinii/f  (4.C.3) 

ky-  =  Vf  sin  6f 

and 

Xy  =  Vf  cos  0f  cos  f  -  OfVf  sin  0 f  cos  xY  f  -  x¥  fVf  cos  0f  sin  T  f 

xfj  =  Vf  cos  Of  sin  'P/  -  0fVf  sin  0f  sin  'P  f  +  ¥  fVf  cos  0f  cos  'P  f  (4.C.4) 

xSy  =  Vf  sin  0f  +  OfVf  cos  0f 

In  order  to  ensure  a  smooth  flight  path  at  the  initial  and  final  points,  an  additional 
constraint  of 

■■■SM  _  ■■■SM  _  n 

i  A1  /  u 

x2f  =  x™  =  o  (4.C.5) 

X™=x™=  0 

will  be  imposed  on  the  system  at  the  initial  and  final  conditions.  This  will  have  the 
practical  result  of  limiting  the  initial  and  final  controls  on  the  system  to  smooth 
maneuvers,  thus  avoiding  any  tendancy  to  conduct  a  “flair  maneuver”  at  the  last  second. 
In  current  air-to-air  engagements,  the  kill  capability  of  the  warhead  can  be  greatly 
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improved  by  a  last  minute  change  in  the  orientation  of  the  missile,  a  rapid  jerk  of  the 
attitude  from  the  flight  path  to  an  optimal  angle,  which  is  tenned  a  “flair  maneuver”. 
This  focuses  the  blast  effects  toward  the  target  and  is  of  great  usefulness  when  it  is  the 
warhead  damage  that  kills  the  target;  but  in  this  situation  it  is  the  kinetic  energy  of  the 
missile  impact  that  kills  the  target.  A  flair  maneuver  would  only  dissipate  much  of  that 
energy,  possibly  all  of  it,  in  order  to  achieve  the  algorithm’s  desired  impact  angle  and 
render  the  developed  flight  path  useless  for  the  mission.  Similarly,  an  immediate  and 
large  maneuver  at  the  start  of  the  flight  path  would  only  dissipate  the  energy.  Such 
maneuvers  are  discarded  by  these  beginning  and  ending  boundary  conditions. 


2.  Separating  and  Recombining  Space  and  Time 

As  discussed  previously,  in  order  to  independently  optimize  the  spatial  trajectory 
and  the  velocity,  the  reference  function  will  be  derived  as  a  function  of  r  .  The  boundary 
conditions  cannot,  therefore,  be  defined  as  functions  of  time  derivatives  as  in  equations 
(4.C.3)  -  (4.C.5).  A  connection  between  the  spatial  and  time  domains  must  therefore  be 
introduced,  A ,  which  is  defined  as 

-l«  =  A  (4.C.6) 

dt 


and  is  termed  the  virtual  speed  [Ref  19].  This  allows  for  the  independent  variation  of  the 
speed  profile  along  the  same  paths  according  to  any  other  convenient  reference.  In  this 
case  the  known  thrust  profile, nx,  will  be  utilized  by  integrating  the  third  equation  of 
(2.B.3)  and  applying  the  virtual  speed 


V'(t)  =  g(nx 


sin  6) 


dr 

dt 


g(nx  -  sin  6) 

m 


(4.C.7) 


Further  use  of  the  virtual  speed  allows  the  recalculation  of  the  initial  and  final  boundary 
conditions,  transfonning  them  from  the  time  frame  to  the  spatial  frame.  For  this,  the 
obvious  relations 
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Xl(T)  =  ^^-  =  x'i(T)A(T) 
dr  at 


x,.(r)  = 


d(x'i(r)A(r))  dr 


(4.C.8) 


dr 


dt 


=  x"A  +x/T 


i  =  1,2,3 


which  when  rearranged  defines  the  first  and  a  second  derivative  of  the  missile 
coordinates  as 


x'  =  A  'x,  x”  =  A  2  [x,  -  x;/!/]  i  =  1, 2, 3 


(4.C.9) 


Using  the  values  of  A  and  A'  defined  as 


A  =V 
/L0  v  0  ’ 


V  —VV 
/Lo  y  ov  o 


1  Af=Vf,  A'f=VfV -1 


7  f  /'  / 


(4.C.10) 


3.  Reference  Trajectory 

The  knowledge  of  the  initial  and  final  position  plus  the  initial  and  final  conditions 
of  the  first  and  second  time  derivates  allows  for  the  construction  of  a  7th-order 
polynomial  (the  maximum  orders  of  the  time  derivatives  of  the  missile  coordinates  at  the 
initial  and  final  points  plus  one)  to  describe  the  reference  function  of  the  aircraft 
coordinates  x;  (i  =  1,2,3)  .  The  following  are  introduced  as  the  reference  functions  [Ref 
19]: 


(max(l,k-2))!rA 


k\ 


xi{r)  =  YJaik 

k= 0 

X\  (Q  =  ^  atl  (max(1,  ^  -  2» 1  r*  ‘ 


k= 1 
5 


(k-l)! 


aikr 


k-2 


<'(t) = x 

k= 2 

^j)  =  YJ(k-2)alkr 


k- 3 


k= 3 


(4.C.11) 


Or  written  another  way, 
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x"',.(r)  =  ai3  +  aj4z  +  ai5z 2  +  ai6z 3  +  a,7r4 
x  "i  (r)  =  a  i2  +  ai3r  +  y  a,.4r 2  +  ^  a,.5r3  +^-a,.6r4  +^-ai7r5 

x';.(r)  =  afl  +a,.2r  +  ^a;.3r2  +  ^a,-4r3  +-^-«,-5r4  +^-a,.6r5  +^~anr6 
2  6  12  20  30 

x/(r)  =  a;.0  +  anr +  -a;2r2  +-a!3r3  +— a,-^4  +  — a,'5^5  +7T7:a,6r6  +  3m  a/7r7 

2  6  24  60  120  210 

The  coefficients  can  be  detennined  by  solving  the  equations  simultaneously, 


(4.C.12) 
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(4.C.13) 


Finally,  by  substituting  the  corresponding  values  of  xj0,x'i0,xj0  (i  =  1,2,3)  for 
r0  =0,  and  xf , x'f , x"f  (i  =  1,2, 3)  for  z  =  z  f  (where  zf  is  the  first  optimization  parameter, 
the  virtual  arc),  results  in  a  set  of  24  linear  algebraic  equations  for  21  unknown 
coefficients  aik  (7  =  1, 2, 3,  A:  =  0,1,.., 7) 

_  _  r  _  tt  _  rtf 

ai0  ~  Xi 0  ai\  ~  Xi0  ai2  ~  Xi 0  “i'3  —  X 

_  -4x”  +  164  60x"  - 1 2O.4  -3604-4804,  840x,y  -840x,0 

ai4  =  1  2  1  3  1  4 

Ty  T  j-  T  j-  T  y 

30.1-;;  +  6O.4  -4204  +  600  4  23404  -  27004  -5040x,r  +  5040.x, 0 

al5  = - ^  + - 4 - ^  + - *— - li  + - 4 - —  (4.C.14) 


-6O.4  -  804  7804  -  9004  -40804  -  43204  8400x, r  -  8400x, 0 

a«= - 3 - + - 3 - + - 3 - + - 3 - 

Ty  Ty  Ty  T  j- 

354  +  354  -4204  +  420x,"0  2 1  OO.x,'0  +  2 1 00x7  -4200x(/  +  4200x,0 

n  = - - - 1 - - - 1 - —  -I - - - 

11  _4  '  5  ^  6  ^  7 

Ty  Ty  T^  T^ 
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4.  Inverse  Dynamics 

The  numerical  solution  develops  a  reference  trajectory  over  a  fixed  set  of  N points 
equidistantly  placed  along  the  virtual  arc.  The  virtual  interval  is 


At  -■ 


N- 1 


and  the  corresponding  time  interval  is 


Atj  =  2 


Z  ~xrj- 1) 

V  1  J 


1 

^2 


y  =  l,2,...,7V-l 


(4.C.15) 


(4.C.16) 


VJ+VH 

where  Fy  and  F/_l  is  determined  by  integrating  equation  (4.C.7).  iV  is  any  convenient 
number,  chosen  to  be  100  in  this  program. 

The  value  of  Vj  also  allows  for  the  determination  of  both  0  and  T  by  rearranging 
equations  (2.B.2)  and  substituting  the  virtual  velocity 


6  =  arctg 


/  t 

VX  i; 


2  .  t  2 

J  +X2  J 


Vj  =  arctg  — 


2;y 


xi;y 


The  controls  nv  and  n_  are  found  by  rearranging  equations  (2.B.3)  to  be 


(4.C.17) 


V.  . 

nv.  =—  VF,  COS0! 

y’J  g  J 

nrj  =—  (0j  +  cos0 '■) 
g 


where  the  angular  derivatives  are  determined  as 


6j  =  cos‘  6 


2  X"(X'l  +X2  )~X'i  (Xl"  +  X2  ) 


(x[2+x'22f12 


(4.C.18) 


(4.C.19) 
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5.  Cost  and  Penalty  Functions 

Finally,  the  calculation  of  the  flight  path  results  in  a  set  of  functions  that  must  be 
minimized,  which  occurs  through  a  Cost  Function  (CF)  and  a  Penalty  Function  (PF). 
These  functions  must  be  carefully  chosen  to  ensure  that  the  optimal  path  is  truly  feasible, 
desirable,  and  obtainable.  A  simple  example  of  a  CF  is  J  =  tf ,  the  minimal  time 

problem,  or  Js  |«(7)| ,  the  minimum  fuel  problem,  though  there  is  no  limit  to  the  number 
or  variation  of  the  Cost  Function. 


In  this  case,  the  CF  was  chosen  to  optimize  three  properties  simultaneously; 
minimize  the  length  of  the  virtual  arc,  r/ ,  minimize  the  time  to  intercept,  t  ,  and 

maximize  the  impact  angle  of  the  interception.  The  CF  for  this  program  is  written  as 


J  =  wxkxTf  +  w2k2tgo  +  w3k3 


VBR  ■ V 


SM 


(4.C.20) 


Each  item  must  be  scaled  appropriately  using  the  scaling  factors  kl,k2,k3,  so  that 

they  are  all  roughly  equivalent  when  optimized,  e.g.  the  anticipated  intercept  time  is 
counted  in  tens  of  seconds  while  the  cosine  of  the  impact  angle  will  vary  from  zero  to 
one.  Failure  to  weight  them  properly  will  skew  the  results  of  the  cost  function. 
Additionally,  through  the  weighting  functions  wx,w2,w3,  a  trade-off  analysis  can  be 
conducted  and  variables  can  be  included  or  excluded  as  desired. 

The  first  two  variables,  r  and  t  ,  are  necessary  to  ensure  the  system’s  optimal 

solution  is  actually  physically  realizable.  Without  them,  the  system  will  continue  to 
optimize  the  intercept  well  beyond  the  capabilities  of  the  missile  or  even  physical  reality. 
One  example  is  that,  in  a  purely  mathematical  sense,  there  is  no  problem  with  a  negative 
velocity  (yet  in  the  physical  world  that  makes  no  sense)  and  the  program  might  try  a 
program  that  would  intercept  after  the  missile  velocity  has  gone  past  zero  and  into 
negative  numbers  (of  course,  the  missile  would  stop  flying  long  before  it  even  reaches 
zero).  One  might  be  tempted  to  include  a  myriad  of  parameters  to  cover  all  possible 
eventualities;  however,  including  these  two  parameters  sufficiently  accounts  for  nearly 
every  physical  limitation  and  eliminates  the  need  for  having  a  long  list  of  cost  variables. 
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The  third  variable  in  the  CF  was  chosen  in  order  to  maximize  the  angle  of  impact 
upon  interception.  This  reflects  the  need,  described  in  Chapter  1,  to  maximize  the  kinetic 
energy  in  order  to  ensure  the  interceptor  disables  the  target.  The  cost  is  calculated  using 
a  simple  dot  product  relationship 


cos# 


dot(xsfM  ,xBfR) 


ZSM 

Zbr 

Xf 

xf 

(4.C.21) 


which  will  have  a  minimum  value  of  zero  when  the  impact  angle  is  at  a  maximum. 

The  PF  is  chosen  to  ensure  that  certain  conditions  are  not  violated  or  exceeded, 
such  as  physical  limitations.  In  this  case,  the  PF  is  on  the  maximum  acceleration  in  the  y 
and  z  direction.  Zarchan  showed  that  acceleration  capability  is  dependent  on  altitude  and 
speed  [Ref  21].  The  PF  has  been  set  up  to  reflect  this  by  varying  between  40  g’s  at  sea 
level  to  10  g’s  at  50,000  ft. 

These  are  not  the  only  CF  or  PF  variables  that  can  be  included.  The  choice  of 
variables  to  include  is  situation  dependant  and  may  be  modified  to  meet  whatever  the 
needs  of  the  situation  demand. 


4.  Simulation  Outline 

The  simulation  begins  with  the  definition  of  the  ballistic  missile  flight  path.  The 
data  will  be  used  to  represent  what  the  missile  “sees”  when  it  is  defining  the  boundary 
conditions.  A  perfect  picture  is  assumed  for  this  simulation.  Once  the  ballistic  flight 
path  has  been  defined,  the  interceptor  is  launched.  The  interceptor  launch  point  was 
chosen  as  [100000;  100000;  Re]r  ,  roughly  150  nm  from  the  launch  position. 

The  first  portion  of  the  SM-6  flight  is  the  vertical  launch,  which  was  assumed  to 
last  6  s,  followed  by  a  4  s  period  in  which  the  missile  arcs  over  toward  the  target,  as  it 
would  do  under  guidance  from  the  AEGIS  weapon  system  in  normal  circumstances.  At 
10  seconds,  the  state  of  the  interceptor  and  the  state  of  the  ballistic  target  is  “seen”  and 
input  into  the  system.  At  this  point  the  missile  would  not  have  an  independent  radar 
picture  of  the  target,  so  instead  would  be  fed  targeting  data  from  the  AEGIS  weapon 
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system.  This  can  continue  indefinitely  until  the  interceptor  has  its  own  radar  fix  on  the 
ballistic  missile. 


A  first  “guess”  at  time  to  go  and  flight  distance  is  done  by  a  simple  iterative 
process  that  takes  the  known  velocity  profile  and  iterates  an  initial  intercept  time  using  a 
first  order  approximation  of  the  ballistic  path.  This  process  uses  the  known  velocity 
profile  of  the  SM-6  and  the  known  velocity  profile  of  the  TD-2  in  order  to  develop  an 
accurate  “guess”  according  to 


tgoi  =  100;  £  =  5; 
while  8  >  I 
z/  tgol  <  26 

3500 

r<-^' 

v  =Vj- 

ave  2 


else 


Vf=  3500-10(7gol  -26) 


V  =(26,3500  +  (^1-26)(3500  +  f7) 


2 1 


gol 


end 


xf=xt+xttgol 


lgo  2 


J 


X(xf,,  -  A,;/)2 

!=1 


\\xdt  \\  +  V„, 


5  -  t  r,  ~  t  , 

go  2  1 


tgo\ 


t  .  t  n 

go 1  go 2 


end 


t  =  t  , 

*go  gol 


Also,  the  program  creates  an  initial  guess  at  the  optimum  final  values  of  the  flight 
path  angle  and  heading  using  (4.C.3).  This  serves  only  as  an  initial  start  point  and  must 
only  be  mildly  accurate,  since  the  program  will  optimize  the  time  and  flight  path  as  it 
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operates.  The  accuracy  of  the  initial  guesses  only  serves  to  decrease  the  necessary 
computation  time. 

The  MATLAB  function  “fminsearch”  carries  out  the  optimization  by  varying  the 
four  optimization  parameters  and  evaluating  the  cost  function.  Once  the  minimum  value 
of  the  CF  has  been  found,  the  fminsearch  returns  the  required  control  time  history  to  the 
missile  guidance  system,  which  can  then  execute  the  commands  and  fly  the  derived  flight 
path.  Since  the  missile  system  can  be  programmed  with  sufficient  data  to  compensate  for 
its  control  system  time  constant,  the  system  lag  can  be  effectively  negated,  thus 
eliminating  a  source  of  error. 

Updating  the  guidance  system  every  several  seconds  will  result  in  increasingly 
accurate  final  intercept  positions  and  further  ensure  the  mission  kill. 


D.  SIMULATION  RESULTS 

The  simulation  ran  as  expected,  testing  about  80  iterations  and  different  flight 
paths  before  arriving  in  the  neighborhood  of  the  final  value,  and  testing  about  160 
iterations  and  different  flight  paths  before  arriving  at  the  optimal  solution. 

The  three  coordinate  axes  were  independently  optimized.  Figures  28-30  show  the 
flight  paths  generated  and  the  first  and  second  derivatives  of  those  flight  paths  in  each  of 
the  coordinate  axes.  It  is  clear  that  each  flight  path  is  smooth,  continuous,  and  satisfies 
the  initial  and  final  boundary  conditions. 


64 


"O 


10  15  20  25  30  35  40  45  50 


time  (s) 

Figure  30.  Z-axis  Flight  Path  and  Derivatives 

It  is  important  to  note  that  the  derivatives  of  the  flight  path  are  not  the  velocity 
and  acceleration  in  a  time-frame  sense,  but  instead  are  the  spatial  derivatives.  They 
represent  the  rate  of  change  of  the  motion  of  the  flight  path.  In  an  abstract  sense,  the 
interceptor  missile  could  travel  along  this  optimum  path  at  any  chosen  speed  (though  in 
this  case  the  velocity  history  is  defined),  and  still  be  flying  along  the  optimum  path.  The 
velocity  history  is  introduced  after  the  optimum  path  has  been  derived  to  detennine  the 
flight  time  and  test  the  boundary  conditions. 

The  final  flight  path  selected  is  shown  in  Figures  3 1-33,  in  three  dimensional  plots 
from  various  angles.  In  each  figure  is  a  large  black  star  designating  the  point  that  the 
interceptor  activated  the  guidance  law  and  the  point  that  the  state  of  the  ballistic  rocket 
was  extracted.  The  only  state  that  the  interceptor  missile  was  given  was  the  one 
represented  by  the  star.  The  final  intercept  position  is  on  the  ballistic  flight  path  because 
the  algorithm  optimized  the  intercept  time  and  detennined  the  final  position  based  on  that 
optimization.  This  is  the  major  difference  between  this  program  and  most  predictive 
intercept  guidance  laws  where  the  time  to  intercept  is  only  an  educated  guess. 
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Figure  31.  3D  Optimal  Flight  Path 
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V.  CONCLUSIONS 


A.  VERIFICATION  OF  OPTIMAFITY 


A  plot  of  the  position  and  velocity  of  the  ballistic  missile  overlaid  upon  the  final 
optimal  flight  path  shows  the  final  intercept  geometry,  as  shown  in  Figure  34,  where  the 
lines  extending  from  the  position  point  represent  the  current  velocity  vector  at  that 
moment  (the  relative  scale  shows  the  relative  speeds  and  the  circle  at  the  end  denotes 
“forward”  for  each).  It  is  clear  from  Figure  34  that  the  interceptor  does  indeed  intercept 
the  ballistic  missile,  and  at  a  near  right  angle  as  was  intended, 
x  10® 


6.48 


- 


-2 


x  10 


F igure  33.  F inal  Interception  Geometry 


Another  way  to  verify  that  the  interception  is  as  predicted  is  to  look  at  each 
coordinate  axis,  as  shown  in  Figures  35-37.  Again  the  missile’s  heading  and  relative 
speeds  are  denoted  by  the  lines  extended  from  the  position  point,  and  the  line  shows  the 
optimum  flight  path. 
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Figure  35.  2D  (X-Z  axis)  Optimum  Flight  Path 
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Figure  36.  2D  (Y-Z  axis)  Optimum  Flight  Path 

It  is  clear  that  the  intercept  does  occur  and  that  it  is  at  a  near  right  angle.  The 
remaining  question  is  whether  it  is  feasible  and  physically  possible  for  the  interceptor 
missile  to  fly  the  derived  path.  Figure  38  shows  the  time  history  of  the  heading  and  flight 
path  angles. 
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Figure  37.  Heading  ( Y  )  and  Flight  Path  Angle  ( 9  )  Time  History 

It  is  clear  from  Figure  38  that  the  path  generated  does  not  require  large  variations 
of  heading  or  flight  path,  suggesting  it  is  a  feasible  flight  path.  However,  this  is  not 
conclusive.  Instead,  the  required  axial  forces  must  be  verified  to  be  constantly  within  the 
limitations  of  the  system.  Figure  39  shows  the  time  history  of  the  independent  control 
forces,  n  ,nz.  The  dashed  line  represents  the  maximum  capability  of  the  system  at  each 

point,  based  on  the  altitude  of  the  missile  as  discussed  in  Chapter  IV. 
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Figure  38.  Control  Forces  Time  History 

It  is  clear  from  Figure  39  that  the  missile  is  capable  of  navigating  the  generated 
flight  path.  Note  that  it  uses  the  full  capabilities  of  the  missile  to  conduct  the 
interception,  which  suggests  that  the  program  is  appropriately  accounting  for  the  missile 
capabilities.  Also  note  that  the  required  forces  are  near  zero  at  the  intercept  point, 
demonstrating  no  flair  maneuver  or  high-g  maneuver  was  conducted.  This  suggests  that 
the  full  kinematic  energy  of  the  missile  is  directed  into  the  target,  which  was  the  original 
intent  of  the  program. 

It  can  be  proven  that  the  penalty  functions  were  indeed  influencing  the  choice  of 
trajectory.  If  the  required  control  effort  exceeded  the  maximum  capabilities  of  the 
missile  the  trajectory,  the  path  should  be  rejected  as  infeasible.  It  can  be  seen  from 
Figure  40  that  this  is  the  case,  and  that  the  final  path  has  no  penalty  assigned  to  it. 
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Figure  39.  Penalty  Function  Values 

The  flight  path  is  feasible,  it  accomplishes  the  intended  goal,  and  is  within  the 
capabilities  of  the  interceptor  missile.  The  final  question,  then,  is  whether  the  generated 
flight  path  is  in  fact  “optimal”,  i.e.  the  best  available  path.  Figure  41  shows  the  value  of 
the  CF  after  each  iteration,  with  emphasis  on  the  final  series  of  values.  The  figure  clearly 
demonstrates  that  the  value  of  the  CF  quickly  gets  within  the  neighborhood  of  the 
minimum,  and  after  a  short  duration  reaches  the  minimum  cost  function. 
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Figure  40.  Iterative  Value  of  the  Cost  Function 

It  is  also  important  to  ensure  the  impact  angle  is  maximized.  It  has  already  been 
shown  from  the  various  graphs  that  the  impact  angle  is  indeed  close  to  90  degrees.  It 
remains  to  be  seen,  however,  if  it  the  best  one  that  can  be  found.  It  is  possible  that  this 
flight  path  is  returned  as  “optimal”  not  because  it  maximizes  the  impact  angle,  but  instead 
because  the  values  of  tg0  and  r/  dominate  to  the  point  that  impact  angle’s  contribution  is 

negligible.  Figure  42  shows  the  value  of  the  impact  angle  after  each  iteration  of  the 
program,  with  emphasis  on  the  final  values. 
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Figure  4 1 .  Iterative  Value  of  the  Cosine  of  the  Impact  Angle 

The  two  remaining  variables  to  be  optimized  have  no  target  value,  as  there  was 
with  the  impact  angle.  It  therefore  must  only  be  shown  that  the  algorithm  has  indeed 
tried  numerous  values  and  found  the  best  one.  Figure  43  shows  the  iterative  value  of  tg0, 
and  clearly  shows  this  is  occurring.  Likewise  for  Figure  44  regarding  zf . 
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Figure  42.  Iterative  Value  of  Intercept  Time 
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Figure  43.  Iterative  Value  of  r f 


It  can  therefore  be  said  that  the  generated  flight  path  is  indeed  the  solution  which 
minimizes  the  flight  path  distance,  minimizes  the  time  of  flight,  and  maximizes  the 
impact  angle  -  the  optimum  one  for  this  intercept. 

B.  APPLICABILITY 

This  algorithm  can  easily  be  incorporated  into  existing  control  systems  for 
numerous  applications.  Yakimenko  has  shown  that  for  short  term  spatial  maneuvers,  the 
system  is  extremely  robust  and  capable,  and  requires  only  a  moderate  level  of 
computational  power  [Ref  19].  The  power  required  for  this  particular  application  is 
slightly  more  since  the  final  boundary  conditions  are  not  fixed  but  instead  vary  with  the 
time  and  flight  path  conditions.  Still,  a  dedicated  system,  such  as  onboard  a  properly 
modified  SM-6,  must  be  capable  of  not  only  deriving  the  flight  path,  but  also  storing  the 
data  and  executing  it.  None  of  these  are  particularly  challenging  requirements,  as  many 
other  missiles  in  the  US  inventory  complete  many  of  these  individual  tasks  already. 

This  program  cannot  take  into  account  the  future  actions  of  the  target.  It  is  thus 
not  applicable  to  a  target  that  is  capable  of  maneuvering  or  deviating  from  its  flight  path 
in  a  considerable  manner.  This  limits  the  scope  and  applicability  of  the  program,  but 
there  are  currently  many  very  capable  guidance  laws  for  these  situations.  The  purpose 
here  was  to  propose  a  law  that  is  very  specific  in  its  application  but  which  operates  far 
beyond  the  capabilities  of  otherwise  excellent  guidance  laws. 


C.  SUGGESTIONS  FOR  FUTURE  RESEARCH 

Several  avenues  remain  unexplored  within  this  area  of  study.  Some  weaknesses 
that  may  be  overcome  by  further  research  are: 

1 .  Perform  a  trade-off  analysis  of  cost  function  weighting  coefficients  to 
determine  if  the  chosen  ones  are  the  most  influential  choices,  or  if  they  are 
redundant,  or  if  other  choices  may  provide  better  results. 

2.  Implement  a  second-order  prediction  model  of  the  Ballistic  Missile  path  in  the 
development  of  the  final  boundary  conditions  to  improve  the  accuracy  of  the 
interception. 
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3.  Perform  robustness  analysis  using  the  developed  6DOF  high-fidelity  model 
and  auto-pilot  model  in  the  presence  of  disturbances. 

4.  Perfonn  Monte  Carlo  simulation  on  launch-interception  scenario  to  determine 
the  limitations  on  the  algorithm. 
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APPENDIX  A:  MODELING  PROGRAMS 


A.  3DOF  TARGET 

1.  BRParams3.m 

function  [BR  mass, dia, length] =BRParams3 (t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

This  function  calculates  the  mass  of  the  rocket,  assuming  a  cruciform 
rocket  in  two  stages  plus  an  unpowered  nosecone  stage.  This  function 
also  returns  the  reference  (base)  diameter  of  the  missile. 
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Notes : 

The  first  stage  last  125  seconds,  the  second  stage  lasts  an 
additional  110  seconds.  The  stages  separate  upon  completion. 


Variable  Li 
BR  mass 
BR  nose 
BR~stl_bt 
BR  stl  fcr 
BR  stl  fuel 

BR  stl  str 
BR  stl  tfm 
BR  st2  bt 
BR  st2  fcr 
BR  st2  fuel 

BR  st2  str 
BR  st2  tfm 
dia 
t 


st 

=total  rocket  mass 
=total  mass  of  nosecone  section 
=burntime  for  stage  1 
=consumption  rate  of  stage  1  fuel 
=remaining  stage  1  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  1  structural  material 
=total  mass  of  stage  1  fuel 
=burntime  for  stage  2 
consumption  rate  of  stage  2  fuel 
=remaining  stage  2  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  2  structural  material 
=total  mass  of  stage  2  fuel 
=reference  diameter,  base  diameter 
=time 


Structural  Components 
BR  nose=250; 

BR  st2  str=2288; 

BR  stl  str=9000; 


%%  Fuel  Components 

BR  stl  tfm=50970; 

BR  stl  bt=125; 

BR  stl  fcr=BR  stl  tfm/BR  stl  bt; 

BR  st2  tfm=12912; 

BR~st2Jot=110; 

BR  st2  fcr=BR  st2  tfm/BR  st2  bt; 
if  t<125 

%%  Stage  1  -  Stage  1  Fuel  is  consumed;  Stage  2  Fuel  is  not  used. 
BR  stl  fuel=BR  stl  tfm-BR  stl  fcr*t; 

BR  mass=BR  nose+BR  stl  str+BR  stl  fuel+BR  st2  str+BR  st2  tfm; 

dia=2 . 2 ; 

length=2+14+16; 
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elseif  t<2  4 0 ; 

%%  Stage  2  -  Stage  1  has  separated;  Stage  2  Fuel  is  consumed. 
BR  st2  fuel=BR  st2  tfm-BR  st2  fcr* (t-125) ; 

BR  mass=BR  nose+BR  st2  str+BR  st2  fuel; 
dia=l . 3 ; 
length=2+14 ; 


else 

%%  Stage  3  -  Stage  2  has  separated;  unpowered  nosecone  remains. 
BR  mass=BR  nose; 
dia=l . 3 ; 
length=2 ; 


end 

return 


2.  BRFlight3.m 

%%  Script  File 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  integrates  the  position,  velocity,  and  acceleration 
values 

%  at  each  time  step  to  determine  the  flight  path  of  a  ballistic  missile 
%  in  a  gravity  turn.  The  script  calls  BRParams3.m,  ZLDragC.m,  and 
%  STatmos.m 


%%  Variable 
%  acc 
%  alt 
%  ax 
%  ay 
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CD 

dia 

Drag 

dt 

g 

gm 

i 

m  r 
Mspd 
num  BR 
nx 

press 

px 

PY 

pz 

Re 

ro 

spd 

Sref 

t 


List 

=total  acceleration 
=altitude 

=x  component  of  acceleration 
=y  component  of  acceleration 
=z  component  of  acceleration 
=drag  coefficient 

=reference  diameter  (base  diameter) 

=total  drag  force 
=time  step  interval 

=gravity  force,  based  on  WGS-84  value  of  gravitational 
attraction  and  altitude 
=initial  launch  angle 
=interval  count 
=rocket  mass 

=rocket  speed  in  Mach  (relative  to  local  speed  of  sound) 
=number  of  iterations  conducted  (used  for  plotting) 
=axial  force 

=local  atmospheric  pressure 
=x  component  of  position 
=y  component  of  position 
=z  component  of  position 
=WGS-84  value  for  Earth's  radius 
=local  atmospheric  density 
=rocket  speed  in  m/ s 

=planar  reference  area  (for  drag  calculations) 

=time 
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temp  =local  atmospheric  temperature 

Thrust  =thrust  generated  by  motor 

vx  =x  component  of  velocity 

v y  =y  component  of  velocity 

v z  =z  component  of  velocity 

Acc^BR  =index-based  vector  of  acceleration  values 

Forces_BR=index-based  vector  of  force  values 
Pos  BR  =index-based  vector  of  position  values 
time  BR  =index-based  vector  of  time  values 
Vel  BR  =index-based  vector  of  velocity  values 
All  BR  =index  based  vector  of  all  rocket  values 


%%  Initialize  Variables 
t=0;  dt=0.5;  i=0; 

Re= 6 .378137e6; 


%%  Initial  Conditions 

px=0; 

py=0; 

pz=Re; 

gm=75*pi/180; 
vx=cos (gm) ; 
vy=0; 

vz=sin (gm) ; 


%%  Ballistic  Flight  Path 
for  t=0:0. 5:2219. 5 
i=i+l ; 


%  Speed,  Mach  Number 
spd=norm( [vx;vy;vz] )  ; 
alt=norm ( [px;py;pz] ) -Re; 
if  alt<86000 

[ro, press, temp] =STatmos (alt) ; 

else 

[ro, press , temp] =STatmos ( 86000 )  ; 

end 

Mspd=spd/ sqrt (1 . 402*287*temp) ; 

%  Forces 

g=3.986004418el4/norm( [px; py; pz ] ) A2 ; 
[m_r, dia] =BRParams3 (t) ; 

[CD] =ZLDragC (Mspd, t) ; 

if  t<125 

Thrust=105000*9 .81; 

CD=CD (1) ; 
elseif  t<240 

Thrust=29950*9 .81; 

CD=CD  (1) ; 

else 

Thrust=0 ; 

CD=CD (2) ; 

end 

Sref=pi*diaA2 / 4  ; 
Drag=ro*spdA2*CD*Sref / 2 ; 
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nx= (Thrust-Drag) /m  r; 


%  Accelerations 

g=3.986004418el4*pz/norm( [px;py;pz] ) A3; 
ax=nx*cos (gm) ; 
ay=0  ; 

az=nx*sin (gm) -g; 
acc=norm( [ax;ay;az] ) ; 

%  Collect  Variables 

time_BR ( i , 1 ) =t ; 

Pos_BR ( i , 1 ) =px; 

Pos_BR ( i , 2 ) =py; 

Pos_BR (i , 3) =pz ; 

Pos_BR ( i , 4 ) =norm ( [px; py; pz ] ) ; 

Vel  BR(i,l)=vx; 

Vel  BR(i,2)=vy; 

Vel  BR(i,3)=vz; 

Vel_BR ( i , 4 ) =spd; 

Vel_BR ( i , 5 ) =Mspd; 

Acc  BR(i,l)=ax; 

Acc_BR ( i , 2 ) =ay; 

Acc_BR ( i , 3 ) =az ; 

Acc_J3R  ( i ,  4  )  =acc ; 

Acc  BR(i,5)=nx; 

Forces  BR ( i , 1 ) =Thrust ; 

Forces  BR(i,2)=m  r; 

Forces_BR ( i , 3 ) =Drag; 

%  Time  Step 

px=px+dt*vx; 

py=py+dt*vy; 

pz=pz+dt*vz ; 

vx=vx+dt*ax; 

vy=vy+dt*ay; 

vz=vz+dt*az ; 

num  BR=length (time  BR) ; 

end 

AllBR=[time  BR  Forces  BR  Pos  BR  Vel  BR  Acc  BR] ; 

clear  CD  Drag  Mspd  Re  Sref  Thrust  acc  alt  ax  ay  az 
clear  g  gm  h  i  m  r  nx  press  px  py  pz  ro  spd  t  temp 


dia  dt 
vx  vy  v z 
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B. 


3DOF  INTERCEPTOR 


1.  SMParams3.m 

function  [SM  mass, dia] =SMParams3 (t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

This  function  calculates  the  J  Matrix  and  Mass  of  the  interceptor, 
assuming  a  cruciform  rocket  in  two  stages  to  intercept.  This 
function  also  returns  the  reference  (base)  diameter  of  the  missile. 


%  Notes: 

The  first  stage  last  6  seconds,  the  second  stage  lasts  an  additional 
10  seconds.  Stage  1  (booster)  separates  upon  completion.  Stage  2 
does  not  separate  after  completion 
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Variable  List 
dia 
1 

p  SM  stl  fuel= 
p  SM  st2  fuel= 
p_SMstr 
r 

ro 

ri 

SM  mass 
SM  nose 
SM  stl  fcr 
SM  stl  fuel 

SM_stl_str 
SM  stl  tfm 
SM  st2  fcr 
SM  st2  fuel 

SM_st2_str 
SM  st2  tfm 
t 

th 

V_body 

V  nose  str 
V_nose_str0 

Venose  strl  = 

V_stl_fuel 
V_stl_str 
V_st2  fuel 

V  st2  str 


reference  diameter,  base  diameter 
length,  varies  by  component 
density  of  stage  1  rocket  fuel 
density  of  stage  2  rocket  fuel 
density  of  structural  material 
radius,  varies  by  component 
outer  radius,  varies  by  component 
inner  radius,  varies  by  component 
total  rocket  mass 
total  mass  of  nosecone  section 
consumption  rate  of  stage  1  fuel 
remaining  stage  1  fuel  based  on  time  and 
consumption  rate 

total  mass  of  stage  1  structural  material 
total  mass  of  stage  1  fuel 
consumption  rate  of  stage  2  fuel 
remaining  stage  2  fuel  based  on  time  and 
consumption  rate 
total  mass  of  stage  2 
total  mass  of  stage  2 
time 

structural  thickness 
volume  of  body  structural  material 
volume  of  nosecone  structural  material 
volume  of  nosecone  structural  material, 
intermediate  value 

volume  of  nosecone  structural  material, 
intermediate  value 
volume  of  stage  1  fuel 

volume  of  stage  1  structural  material 

volume  of  stage  2  fuel 

volume  of  stage  2  structural  material 


structural  material 
fuel 


Structural  Components 
p  SMstr=4225; 
th=. 0208; 


%  Mass  of  Nosecone 
1=. 8255; 
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r=0 .34/2; 

V_nose_strO=pi* (1* ( (rA2+lA2) / (2*r) ) A2-lA3/3- ( ( (rA2+lA2) /.  .  . 

(2*r) ) -r) * ( (rA2  +  lA2) / (2*r) ) A2*asin (1/ ( (rA2  +  lA2) / (2*r) ) ) )  ; 
1=. 8255-th; 
r=0 . 34/2-th; 

V_nose_strl=pi* (1* ( (rA2+lA2) / (2*r) ) A2-lA3/3- ( ( (rA2+lA2) /. . . 

(2*r) ) -r) * ( (rA2+lA2) / (2*r) ) A2*asin (1/ ( (rA2+lA2) / (2*r) ) ) ) ; 

V  nose  str=V  nose  strO-V  nose  strl+pi*r A2 *th; 

SM  nose=1.3*V  nose  str*p  SMstr; 

%  Mass  of  Body/Warhead  Section 
1=. 849; 
ro=0.34/2; 
ri=0 . 34/2-th; 

V  body=l*pi* (roA2-riA2)  ; 

SM  body=V  body*p_SMstr+115; 

%  Mass  of  Stage  1  (Mk72  Booster) 

1=1.72; 
ro=0 . 53/2 ; 
ri=0 . 53/2-th; 

V  stl  str=l*pi* (roA2-riA2) +2*pi*roA2*th; 

SM  stl  str=V  stl  str*p  SMstr; 

%  Mass  of  Stage  2  (Mkl04  Engine) 

1=2 . 88-2*th; 
ro=0.34/2; 
ri=0 . 34/2-th; 

V  st2  str=l*pi* (roA2-riA2) +2*pi*roA2*th; 

SM  st2  str=V  st2  str*p  SMstr; 

%%  Fuel  Components 

%  Stage  1  Solid  Fuel  is  HTPB/AP/A1 

1=1.72; 

ri=0 . 53/2-th; 

V  stl  fuel=0 . 80*l*pi*ri A2 ; 
p_SM_stl_fuel=l 8  60 ; 

SM  stl  tfm=468; 

SM_stl_fcr=468/ 6; 

%  Stage  2  Solid  Fuel  is  TP-H1205/6 

1=2 . 88; 

ri=0 . 34/2-th; 

V  st2  fuel=0 . 60*l*pi*ri A2  ; 

SM_st2_tfm=360  ; 

p  SM  st2  fuel=SM  st2  tfm/V  st2  fuel; 

SM_st2_fcr=360/15; 

if  t<6 

%%  Stage  1  -  Stage  1  Fuel  is  consumed;  Stage  2  Fuel  is  not  used. 
SM  stl  fuel=SM  stl  tfm  -  SM  stl  fcr  *  t; 

SM  mass=SM  nose+SM  body+SM  st2  str+SM  st2  tfm+SM  stl  str+... 

SM_stl_fuel ; 
dia=0 . 53 ; 

elseif  t<21 

%%  Stage  2  -  Stage  1  has  separated;  Stage  2  Fuel  is  consumed. 
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SM  st2  fuel=SM  st2  tfm  -  SM  st2  fcr  *  (t-6) ; 

SM  mass=SM  nose+SM  body+SM  st2  str+SM  st2  fuel; 
dia=0 .  34 ; 


else 

%%  Stage  3  -  The  unpowered  nosecone  and  Stage  2  remains. 
SM  mass=SM  nose+SM  body+SM  st2  str; 
dia=0 .  34 ; 


end 

return 


2.  SMFlight3.m 

%%  Script  File 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  develops  and  tracks  the  flight  path  of  the  interceptor 
%  missile.  For  the  first  ten  seconds  it  integrates  a  series  of 
%  acceleration  commands  to  simulate  a  vertical  launch  and  tip  over. 

%  Upon  activation  of  the  guidance  law,  it  sends  the  known  values  to  the 
%  guidance  law  and  receives  back  the  future  time  history  of  the  optimal 
%  flight  path.  This  script  then  implements  that  optimal  path.  It 
%  updates  the  final  conditions  and  recalculates  the  optimal  flight  path 
%  at  an  (modifiable)  interval  of  10  seconds.  The  script  calls 
%  SMParams3.m,  ZLDragC.m,  and  STatmos.m 
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Variable  List 

Acc_SM  =index-based  vector  of  acceleration  values 
A11SM  =index  based  vector  of  all  interceptor  values 
alt  =altitude 

CD  =drag  coefficient 

count  =counting  variable  to  determine  guidance  law  update 
interval 

dia  =reference  diameter  (base  diameter) 

dist  =cumulative  distance  traveled 

Drag  =total  drag  force 

Forces  SM=index-based  vector  of  force  values 
g  =gravity  force,  based  on  WGS-84  value  of 

gravitational  attraction  and  altitude 
i  =interval  count 

m  i  =interceptor  mass 

Model  SM  =index-based  vector  of  internal  values 

MV  =interceptor  speed  in  Mach  (relative  to  local  speed  of 

sound) 

N1  =variable  used  to  compare  optimal  vector  length 

N2  =variable  used  to  compare  optimal  vector  length 

num  SM  =number  of  iterations  conducted  (used  for  plotting) 

nx  =axial  acceleration  command,  body  frame  x 

nx  Op  =index  based  vector  of  the  optimal  flight  path  values 

ny  =axial  acceleration  command,  body  frame  y 

ny  Op  =index  based  vector  of  the  optimal  flight  path  values 

nz  =axial  acceleration  command,  body  frame  z 

nz  Op  =index  based  vector  of  the  optimal  flight  path  values 
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%  path 
%  Pos_SM 
%  press 
%  psi 
%  psi_old 
%  psi_Op 
%  psidot 
%  psidot^Op 
%  px 

%  pxold 
%  px__Op 
%  py 

%  py_old 
%  py_Op 
%  pz 

%  pz_old 
%  pz_Op 

%  q 
%  Re 
%  ro 
%  spot 
%  spot2 
%  Sref 
%  state 
%  t 

%  target 
%  temp 
%  tgo 
%  th 

%  th_old 
%  th_Op 
%  thdot 
%  thdot_Op 
%  Thrust 
%  time_Op 
%  time  SM 
%  update 
%  V 

%  V_old 
%  V_Op 
%  Vdot 
%  Vdot_Op 
%  Vel_SM 
%  w 

BRFlight3 


=returned  time  history  of  the  optimal  path 
=index-based  vector  of  position  values 
=local  atmospheric  pressure 
=heading  angle 

=heading  angle  (used  in  the  time  step) 

=index  based  vector  of  the  optimal  flight  path  values 
=rate  of  change  of  heading  angle 

=index  based  vector  of  the  optimal  flight  path  values 
=x  component  of  position 

=x  component  of  position  (used  in  the  time  step) 
=index  based  vector  of  the  optimal  flight  path  values 
=y  component  of  position 

=y  component  of  position  (used  in  the  time  step) 
=index  based  vector  of  the  optimal  flight  path  values 
=z  component  of  position 

=z  component  of  position  (used  in  the  time  step) 
=index  based  vector  of  the  optimal  flight  path  values 
=counting  variable  (used  in  another  program) 

=WGS-84  Earth's  radius 

=local  atmospheric  density 

=counting  variable  (used  for  plotting) 

=counting  variable  (used  for  plotting) 

=planar  reference  area  (for  drag  calculations) 

=the  state  of  the  interceptor 
=time 

=the  state  of  the  rocket 
=local  atmospheric  temperature 
=time  to  go  to  intercept 
=flight  path  angle 

=flight  path  angle  (used  in  the  time  step) 

=index  based  vector  of  the  optimal  flight  path  values 
=rate  of  change  of  flight  path  angle 

=index  based  vector  of  the  optimal  flight  path  values 
=thrust  generated  by  motor 

=index  based  vector  of  the  optimal  flight  path  values 
=index-based  vector  of  time  values 
=number  of  updates  to  the  guidance  law  conducted 
=velocity  of  the  interceptor 

=velocity  of  the  interceptor  (used  in  the  time  step) 
=index  based  vector  of  the  optimal  flight  path  values 
=rate  of  change  of  the  velocity 

=index  based  vector  of  the  optimal  flight  path  values 
=index-based  vector  of  velocity  values 
=counting  variable  (used  in  another  program) 


global  tgo  q  states  xf  xpf  spot  spot2  j  path  N1  update  trys 
global  spot  spot2  w  Pos  BR  Pos  SM 

%  Initialize  Variables 
t=0;  dt=0.5; 

i=0;  ii=0;  w=120;  q=0;  qq=0;  rr=0; 
count=30;  update=0; 
spot=13;  spot2=81; 

Re= 6 .378137e6; 
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tgo=85; 

V=1  ; 

psi=-130*pi/180; 

th=90*pi/180; 

V_old=V; 
psi_old=psi ; 
th_old=th; 

px=l 00000 ; 
py=100000 ; 
pz=Re; 
px  old=px; 
py_old=py; 
pz_old=pz ; 
dist=0 ; 

%%  Flight  Path 
for  i=l : 1 : 1000 
t=t+dt; 

%  True  APN  Guidance 
if  t<10 

%  Speed,  Mach  Number 
alt=norm ( [px;py;pz] ) -Re; 
if  alt<86000 

[ro,  press,  temp] =STatmos (alt) ; 

else 

[ro,  press,  temp] =STatmos ( 8 6000 )  ; 

end 

MV=V/ sqrt (1 . 4 02 *2 87 * temp) ; 

%  Forces 

g=3.986004418el4/norm( [px; py; pz ] ) A2 ; 

[m_i, dia] =SMParams3 (t) ; 

[CD] =ZLDragC (MV, t) ; 
if  t<6 

Thrust=2 06500 ; 

CD=CD ( 1 ) ; 
elseif  t<20 

Thrust=95300; 

CD=CD (1) ; 

else 

Thrust=0 ; 

CD=CD (2) ; 

end 

Sref=pi*diaA2 / 4  ; 

Drag=ro*VA2*CD*Sref / 2  ; 
nx= (Thrust-Drag) /m  i/g; 

%  Guidance 

if  t<=6  %  Boost  Phase  Vertical  Launch 

psidot=0 ; 
thdot=0 ; 

ny=V/g*cos (th) *psidot; 
nz=V/g*thdot+cos (th) ; 
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else 


Boost  Phase  Pitch  over 


psidot=0 ; 
thdot=-0 .075; 
ny=V/g*cos (th) *psidot; 
nz=V/g*thdot+cos (th) ; 

end 

%  Kinematics 
Vdot=g* (nx-sin (th) ) ; 
psidot=ny*g/V/cos (th) ; 
thdot=g* (nz-cos (th) ) /V; 

%  Collect  Variables 

time_SM ( i , 1 ) =t ; 

Forces  SM(i,l)=nx; 

Forces_SM ( i , 2 ) =ny ; 

Forces_SM ( i , 3 ) =nz ; 

Model_SM(i, 1) =V; 

Model_SM ( i , 2 ) =Vdot ; 

Model_SM ( i , 3 ) =th; 

Model_SM ( i , 4 ) =thdot ; 

Model_SM ( i , 5 ) =psi ; 

Model_SM (i, 6) =psidot; 

Pos_SM ( i , 1 ) =px; 

Pos_SM ( i , 2 ) =py; 

Pos_SM ( i , 3) =pz ; 

Pos_SM ( i , 4 ) =dist ; 

Vel_SM (i, 1) =V*cos (th) *cos (psi) ; 

Vel_SM ( i , 2 ) =V*cos (th) *sin (psi)  ; 

Vel_SM (i, 3) =V*sin (th) ; 

Acc_SM (i, 1) =Vdot*cos (th) *cos (psi) -V*cos (th) *sin(psi) *psidot 
-V*sin (th) *cos (psi) *thdot; 

Acc_SM ( i , 2 ) =Vdot*cos (th) *sin(psi) +V*cos (th) *cos (psi) *psidot 
+V*sin (th) *sin (psi) *thdot; 

Acc_SM (i, 3) =Vdot*sin (th) +V*cos (th) *thdot; 

%  Time  Step 
V=V  old+Vdot*dt; 
psi=psi_old+psidot*dt ; 
th=th_old+thdot*dt ; 

px=px_old+V*cos (th) *cos (psi) *dt; 
py=py_old+V*cos (th) *sin (psi) *dt; 
pz=pz  old+V*sin (th) *dt; 

dist= (dist+abs (norm ( [px-px_old; py-py_old; pz-pz_old] ) ) ) ; 

V_old=V; 
psi_old=psi ; 
th_old=th; 

px  old=px; 
py_old=py; 
pz_old=pz ; 
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else 


Optimal  Guidance 


if  count==30 

display (' Starting  SMGuidance  Update') 
update=update+l ; 

state= [px;py;pz; V; th;psi; Vdot; thdot;psidot]  ; 
target=[Pos  BR ( i+w, 1 ) ; Pos  BR ( i+w, 2 ) ; Pos  BR(i+w,3); 

Vel_BR(i+w, 1) ;Vel^BR(i+w,  2) ;Vel~BR(i+w,  3) ] 
[path] =SMGuidance (t, state, target,  i)  ; 
if  update==l; 

Nl=length (path ( : , 1 ) ) ;  N2=N1; 

else 

N2=length (path ( : , 1) ) ; 

end 

%  Identify  Variables 

time_Op ( : , update)  =  [path ( : , 1) ; zeros (N1-N2, 1) ]  ; 
px_Op ( : , update) = [path ( : , 2 ) ; zeros (N1-N2 , 1) ] ; 
py_Op ( : , update) = [path ( : , 3) ; zeros (N1-N2, 1) ] ; 
pz_Op ( : , update)  =  [path ( : , 4) ; zeros (N1-N2, 1) ]  ; 

V_Op ( : , update) = [path ( : , 5) ; zeros (N1-N2, 1) ] ; 
th_Op ( : , update) = [path ( : , 6) ; zeros (N1-N2, 1) ] ; 
psi_Op ( : , update)  =  [path ( : , 7) ; zeros (N1-N2 , 1) ]  ; 
Vdot_Op ( : , update) = [path ( : , 8) ; zeros (N1-N2, 1) ] ; 
thdot_Op ( : , update)  =  [path ( : , 9) ; zeros (N1-N2 , 1) ] ; 
psidot_Op ( : ,  update)  =  [path ( : ,  10) ; zeros (N1-N2, 1) ] ; 
nx_Op ( : , update)  =  [path ( : , 11) ; zeros (N1-N2, 1) ]  ; 
ny_Op ( : , update)  =  [path ( : , 12 ) ; zeros (N1-N2 , 1) ]  ; 
nz_Op ( : , update) = [path ( : , 13) ; zeros (N1-N2, 1) ] ; 
count=l ; 

end 


t=time_Op (count, update)  ; 
nx=nx_Op (count, update)  ; 
ny=ny_Op (count, update)  ; 
nz=nz_Op (count, update)  ; 

V=V_Op (count, update)  ; 
Vdot=Vdot_Op (count, update) ; 
th=th_Op (count, update) ; 
thdot=thdot_Op (count,  update)  ; 
psi=psi_Op (count, update)  ; 
psidot=psidot_Op (count,  update)  ; 
px=px_Op (count, update) ; 
py=py_Op (count,  update)  ; 
pz=pz_Op (count, update)  ; 

%  Collect  Variables 

time_SM ( i , 1 ) =t ; 

Forces  SM(i,l)=nx; 

Forces_SM ( i , 2 ) =ny ; 

Forces_SM ( i , 3 ) =nz ; 

Model_SM(i, 1) =V; 

Model_SM ( i , 2 ) =Vdot ; 

Model_SM ( i , 3 ) =th; 

Model_SM ( i , 4 ) =thdot ; 

Model_SM ( i , 5 ) =psi ; 

Model_SM (i, 6) =psidot; 

Pos_SM ( i , 1 ) =px; 


91 


Pos_SM ( i , 2 ) =py; 

Pos_SM ( i , 3) =pz ; 

Pos_SM ( i , 4 ) =dist ; 

Vel_SM ( i , 1 ) =V*cos ( th) *cos (psi )  ; 

Vel_SM ( i , 2 ) =V*cos (th) *sin (psi) ; 

Vel_SM (i, 3) =V*sin (th) ; 

Acc_SM ( i , 1 ) =Vdot*cos (th) *cos (psi) -V*cos (th) * sin (psi ) *psidot. . . 
-V*sin (th) *cos (psi) *thdot; 

Acc_SM ( i , 2 ) =Vdot*cos (th) *sin(psi) +V*cos (th) *cos (psi) *psidot. . . 

+V*sin (th) *sin (psi) *thdot; 

Acc_SM (i, 3) =Vdot*sin (th) +V*cos (th) *thdot; 

Update ( i , 1 ) =update ; 

%  Time  Step 
count=count+l; 

dist= (dist+abs (norm ( [px-px__old; py-py_old; pz-pz_old] ) ) ) ; 

V_old=V; 

psi_old=psi ; 

th_old=th; 

px  old=px; 

py_old=py; 

pz_old=pz ; 

end 

num  SM=length (time  SM)  ; 
end 

A11SM=  [time  SM  Forces  SM  Model  SM  Pos  SM  Vel  SM  Acc_SM  update] ; 


C.  6DOF  MODEL 

1.  BRDetails6.m 

%%  Script  File 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  defines  mass  and  fuel  parameters  of  the  Ballistic  Rocket, 
%  as  extracted  from  several  open  source  materials  relating  to  the 
%  North  Korean  Taepo-Dong  II  missile  project. 

%  This  script  calculates  the  volume  of  structural  material  and 
%  therefore  the  weight  of  the  structural  components,  assuming 
%  structural  steel  as  the  primary  structural  material.  The  process  to 
%  determine  the  average  thickness  of  the  structure  was  an  iterative 
%  process  based  on  the  known  information  of  launch  weight,  some  fuel 
%  data,  payload,  and  burn  times. 


%%  Variables 
%  BR_fuel_p 
%  BR_str_p 
%  BR  nose 
%  BR__stl_bt 
%  BR  stl  fcr 
%  BR  stl  str 
%  BR  stl  tfm 


List 

=density  of  rocket  fuel 

=density  of  structural  material 

=total  mass  of  nosecone  section 

=burn  time  of  stage  1  fuel 

consumption  rate  of  stage  1  fuel 

=total  mass  of  stage  1  structural  material 

=total  mass  of  stage  1  fuel 
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%  BR  st2  bt  =burn  time  of  stage  2  fuel 

%  BR  st2  fcr  consumption  rate  of  stage  2  fuel 

%  BR  st2  str  =total  mass  of  stage  2  structural  material 

%  BR  st2  tfm  =total  mass  of  stage  2  fuel 

%  1  =length,  varies  by  component 

%  r  =radius,  varies  by  component 

%  ri  cuter  radius,  varies  by  component 

%  ro  =inner  radius,  varies  by  component 

%  tctc  =total  vs.  chamber  thrust  comparison,  used  to 

%  extrapolate  fuel  consumption  rate  for  stage  1 

%  from  stage  2 

=thickness  of  structural  material 
str  =volume  of  nosecone  structural  material 
strO=volume  of  nosecone  structural  material, 
intermediate  value 

volume  of  nosecone  structural  material, 
intermediate  value 

V_stl  str  =volume  of  stage  1  structural  material 

V  st2  str  =volume  of  stage  2  structural  material 


th 

V  nose 

V  nose 


V  nose  strl= 


global  BR  str  p  BR  fuel  p  BR  nose  BR  st2  str  BR  stl  str 
global  BR  st2  tfm  BR  stl  tfm  BR  stl  fcr  BR  st2  fcr 

%%  Structural  Components 
BR  str  p=7682; 
the  012; 


%%  Mass  of  Nosecone 
1=2; 

r=l .3/2; 

V_nose_strO=pi* (1* ( (rA2+lA2) / (2*r) ) A2  +  lA3/3- ( ( (rA2  +  lA2)  / (2*r)  )  -r)  * 
(  (rA2  +  lA2) / (2*r) ) A2*asin(l/ ( (rA2  +  lA2) / (2*r) ) ) ) ; 

1=2 -th; 
r=l . 3 /2-th; 

V_nose_strl=pi* (1* ( (rA2+lA2) / (2*r) ) A2+lA3/3- ( ( (rA2+lA2) / (2*r) ) -r) * 

( (rA2  +  lA2) / (2*r) ) A2*asin (1/ ( (rA2  +  lA2) / (2*r) ) ) )  ; 

V  nose  str=V  nose  strO-V  nose  strl; 

BR  nose=250+V  nose  str*BR  str  p; 

%%  Mass  of  Stage  2 

1=14; 
r0=l . 3/2 ; 
ri=l . 3/2-th; 

V  st2  str=l*pi* (rOA2-riA2)  ; 

BR  st2  str=V  st2  str*BR  str  p; 

%%  Mass  of  Stage  1 

1=16; 
r0=2.2/2; 
ri=2 . 2 / 2-th; 

V  stl  str=l*pi* (rO A2-ri A2  )  ; 

BR  stl  str=V  stl  str*BR  str  p; 

%%  Fuel  Components 
BR_fuel_p=801 .164+1544 . 9/4 . 05; 
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%  Fuel  mass,  rate,  burntime 
BR  st2  tfm=12912; 

BR~st2~bt=110; 

BR  st2  fcr=BR  st2  tfm/BR  st2  bt; 

tctc=103000/304327 

BR  stl  fcr=BR  st2  fcr*tctc; 

BR  stl  bt=125; 

BR  stl  tfm=BR  stl  fcr*BR  stl  bt; 


2.  BRParams6.m 

function  [J,BR  mass, dia, length] =BRParams6 (t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  calculates  the  J  Matrix  and  Mass  of  the  rocket, 

%  assuming  a  cruciform  rocket  in  two  stages  plus  an  unpowered  nosecone 
%  stage.  This  function  also  returns  the  reference  (base)  diameter  of 
%  the  missile. 


%%  Notes: 

%  All  CGs  are  measured  from  the  tip  of  the  nosecone. 

%  The  rocket  is  divided  into  five  components:  the  nosecone  and  payload, 
%  stage  one  structure,  stage  one  fuel,  stage  2  structure,  stage  2  fuel. 
%  Liquid  fuel  is  assumed  to  remain  at  the  rear  of  the  fuel  tanks.  Both 
%  stages  utilize  the  same  fuel.  Fuel  is  consumed  at  a  constant  rate. 

%  The  first  stage  last  130  seconds,  the  second  stage  lasts  an 
%  additional  110  seconds.  The  stage  separates  upon  completion. 

%  No  Parallel  axis  theorem  is  required  for  Jx  due  to  symmetry.  Parallel 
%  axis  theorem  is  required  from  CG  for  Jy  and  Jz. 
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Variable  List 


BR  fuel  L 
BR  fuel_p 
BR  mass 
BR  nose 
BR  str  p 
BR  stl  fcr 
BR  stl  fuel 

BR  stl  str 
BR  stl  tfm 
BR  st2  fcr 
BR  st2  fuel 

BR  st2  str 
BR  st2  tfm 
CG 

CG_nose 

CG_stl 

CG_st2 

dia 

J 

Jx 


=Length  of  remaining  fuel  in  the  stage 
=density  of  rocket  fuel 
=total  rocket  mass 
=total  mass  of  nosecone  section 
=density  of  structural  material 
consumption  rate  of  stage  1  fuel 
=remaining  stage  1  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  1  structural  material 
=total  mass  of  stage  1  fuel 
consumption  rate  of  stage  2  fuel 
=remaining  stage  2  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  2  structural  material 

=total  mass  of  stage  2  fuel 

=total  rocket  center  of  gravity 

=nosecone  center  of  gravity 

=stage  1  center  of  gravity 

=stage  2  center  of  gravity 

=reference  diameter,  base  diameter 

=Matrix  of  moment  of  inertia 

=total  rocket  moment  of  inertia,  x-axis 
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%  Jx_nose 
%  Jx_stl_fuel 
%  Jx_stl_str 
%  Jx_st2_fuel 
%  Jx_st2_str 
%  Jy 

%  Jy_nose 
%  Jy_stl_fuel 
%  Jy_stl_str 
%  Jy_st2_fuel 
%  Jy_st2_str 
%  Jz 


=nosecone  moment  of  inertia,  x-axis 
=stage  1  fuel  moment  of  inertia,  x-axis 
=stage  1  structure  moment  of  inertia,  x-axis 
=stage  2  fuel  moment  of  inertia,  x-axis 
=stage  2  structure  moment  of  inertia,  x-axis 
=total  rocket  moment  of  inertia,  y-axis 
=nosecone  moment  of  inertia,  y-axis 
=stage  1  fuel  moment  of  inertia,  y-axis 
=stage  1  structure  moment  of  inertia,  y-axis 
=stage  2  fuel  moment  of  inertia,  y-axis 
=stage  2  structure  moment  of  inertia,  y-axis 
=total  rocket  moment  of  inertia,  z-axis 
=time 


global  BR  str  p  BR  fuel  p  BR  nose  BR  st2  str  BR  stl  str 
global  BR  st2  tfm  BR  stl  tfm  BR  stl  fcr  BR  st2  fcr 


if  t<130 

%  Stage  1  -  Stage  1  Fuel  is  consumed;  Stage  2  Fuel  is  not  used. 
BR  stl  fuel=BR  stl  tfm-BR  stl  fcr*t; 

BR  fuel  L=BR  stl  fuel/ (BR  fuel  p*pi*1.0A2); 
CG_nose=BR_nose*10/ 8; 

CG  stl=BR  stl  str*24+BR  stl  fuel*(32-BR  fuel  L) ; 

CG  st2=BR  st2  str*9+BR  st2  tfm*9; 

BR  mass=BR  nose+BR  stl  str+BR  stl  fuel+BR  st2  str+BR  st2  tfm 
CG= (CG  nose+CG  stl+CG  st2 ) /BR~mass ; 


FF=(BR  stl  fuel+BR  st2  tfm) /BR  mass; 


Jx_nose=BR_nose*3* (0 . 65A2) /10; 

Jx  stl  str=BR  stl  str* (1 . 1A2+1 . 025A2) /2; 

Jx  stl  fuel=BR  stl  fuel* ( 1 . 025 A2 ) /2 ; 

Jx  st2  str=BR  st2  str* ( 0 . 65 A2+0 . 575 A2 ) /2 ; 

Jx  st2  fuel=BR  st2  tfm* (0 . 575A2) /2; 

Jx=Jx  nose+Jx  stl  str+Jx  stl  fuel+Jx  st2  str+Jx  st2  fuel; 


Jy  nose=BR  nose*3* ( . 25* 1 . 3 A2+2 A2 ) +BR  nose* (CG-1 0/8 ) A2 ; 

JyIstl_str=BR_stl_str* (16a2/12+(1.1a2+1.025a2) /4)+  ... 

BR_stl_str* (CG-24 ) A2; 

Jy  stl  fuel=BR  stl  fuel* (1 . 025A2/4  +  BR  fuel  LA2/12)  +  ... 

BR_stl_fuel* (CG- (32-BR_fuel_L/l2) ) A2; 

Jy  st2  str=BR  st2  str* (14A2/12+ (0 . 65A2+0 . 575A2) /4) +  ... 
BR_st2_str* (CG-9) A2; 

Jy_st2_fue1=BR_st2_tfm* (0 . 575A2/ 4+7A2/12)  +  ... 

BR_st2_tfm* (CG-9) A2; 

Jy=Jy_nose+ Jy_stl_str+ Jy_stl_fuel+ Jy_st2_str+ Jy_st2_fuel ; 
Jz=Jy; 


dia=2 . 2 ; 
length=2+14+16; 

elseif  t<2  4 0 ; 

%  Stage  2  -  Stage  1  has  separated;  Stage  2  Fuel  is  consumed. 
BR  st2  fuel=BR  st2  tfm-BR  st2  fcr*(t-130); 

BR  fuel  L=BR  st2  fuel/ (BR  fuel  p*pi*0 . 575A2) ; 


95 


CG_nose=BR_nose*10/ 8; 

CG  st2=BR  st2  str*9+BR  st2  fuel*(14-BR  fuel  L) ; 

BR  mass=BR  nose+BR  st2  str+BR  st2  fuel; 

CG= (CG_nose+CG_st2y/BR~mass; 

Jx_nose=BR_nose*3* (0 . 65A2) /10; 

Jx  st2  str=BR  st2  str* (0 . 65A2  +  0 . 5 7 5 A 2 ) /2; 

Jx  st2  fuel=BR  st2  fuel*  (0 . 575A2) /2; 

Jx=Jx  nose+Jx  st2  str+Jx  st2  fuel; 

Jy  nose=BR  nose*3* (1 . 3A2/4+2A2) +BR  nose* (CG-10/8) A2; 

Jy  st2  str=BR  st2  str* (14A2/12+ (0 . 65A2+0 . 575A2) /4) +  ... 
BR_st2_str* (CG-9)  A2; 

Jy_st2  fuel=BR  st2  fuel* (0 . 575A2/4+BR  fuel  LA2/12)+  ... 

BR_st2_fuel* (CG- ( 1 6-BR_fuel_L/2 ) ) A2; 
Jy=Jy_nose+ Jy_st2_str+ Jy_st2_fuel ; 

J  z=Jy; 


dia=l . 3 ; 
length=2+14 ; 

else 

%  Stage  3  -  Stage  2  has  separated;  only  the  unpowered  nosecone  remains. 
BR  fuel  L=0; 

CG_nose=BR_nose*10/ 8; 

BR  mass=BR  nose; 

CG= (CG_nose) /BR_mass; 

Jx=BR_nose*3* (0.65A2)  /10; 

Jy=BR_nose*3* (1 . 3A2/4+2A2) +BR_nose* (CG-10/8) A2; 

Jz=Jy; 


dia=l . 3 ; 
length=2 ; 


end 


%%  Combining  the  results  into  output  variables. 
J= [ Jx  0  0 ; 

0  Jy  0; 

0  0  Jz] ; 


return 
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3.  BRFlight6.m 

function  [udot] =BRFlight6 (u) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  integrates  the  position,  velocity,  and  acceleration 
%  values  at  each  time  step  to  determine  the  flight  path  of  a  ballistic 
%  missile  in  a  gravity  turn.  The  script  calls  BRParams6.m,  ZLDragC.m, 

%  and  STatmos.m 


%  Variable 
%  [ACoeff] 
%  Aa 
%  As 
%  Cd 
%  Cl 
%  Cm 
%  CN 
%  Cn 
%  CY 

%  delta_h 
%  dia 
%  dP 
%  dPdot 
%  Drag 
%  dY 
%  dYdot 
%  egm 
%  Force 

%  g 
%  h 
%  J 

%  latgc 
%  latgd 
%  lm  g 
%  Mach 
%  Mspd 
%  N 
%  Nh 
%  P 

%  p 

%  pdot 
%  phi 
%  press 
%  psy 
%  px 

%  py 
%  pz 

%  Q 
%  q 
%  qO 
%  ql 
%  q2 
%  q3 
%  qdot 
%  qnorm 
%  Qro 


List : 

=Vector  of  13  coefficients  returned  from  Aerocoeff.m 

=angle  of  attack 

=sideslip  angle 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=iterative  value  for  determination  of  altitude 

=reference  diameter,  base  diameter 

=Pitch  control  surface  deflection 

=Pitch  control  surface  deflection  derivative 

=Total  drag  force 

=Yaw  control  surface  deflection 

=Yaw  control  surface  deflection  derivative 

intermediate  value  for  gravity  calculations 

=Total  force  on  the  center  of  mass 

=Gravity 

=Altitude  above  the  Earth  surface 
=Mass  Moment  of  Inertia  Matrix 
=Geocentric  Latitude 
=Geodetic  Latitude 
=Celestial  Longitude 
=Ballistic  Rocket  speed  in  Mach 
=Local  Speed  of  Sound 

iterative  value  for  determination  of  altitude 
iterative  value  for  determination  of  altitude 
=Roll  Rate 

=Ballistic  Rocket  Position  vector,  [px;py;pz] 

=Ballistic  Rocket  Position  derivative  vector 

=Euler  Roll  wrt  ECEF 

=atmospheric  pressure 

=Euler  Yaw  wrt  ECEF 

=X  Position 

=Y  Position 

=Z  Position 

=Pitch  Rate 

=quaternion  vector 

=Quaternion ' s  0th  component 

=Quaternion ' s  1st  component 

=Quaternion ' s  2nd  component 

=Quaternion ' s  3rd  component 

=Quaternion  derivative 

=Normalization  of  Quaternion 

=Atmospheric  density 
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g. 

o 

R 

=Yaw  Rate 

o, 

o 

R  b2e 

=Rotation  Matrix 

o, 

o 

R  i2b 

=Rotation  Matrix 

o, 

o 

R  i2e 

=Rotation  Matrix 

o, 

o 

R  n2b 

=Rotation  Matrix 

Q. 

o 

Rm 

=Ballistic  Rocket  current  mass 

o, 

o 

ro 

=local  atmospheric  density 

Q„ 

O 

rp 

=iterative  value  for  gravity 

Q. 

O 

speed 

=velocity  of  the  rocket  in  m/s 

O, 

O 

t 

=time 

Q. 

O 

theta 

=Euler  Pitch  wrt  ECEF 

O, 

O 

temp 

=local  atmospheric  temperature 

Q„ 

O 

Thrust 

=thrust  vector 

o, 

o 

Torque 

=torque  vector 

o, 

o 

u 

=input  vector 

o, 

o 

udot 

=output  vector,  to  be  integrated 

o, 

o 

vb 

=velocity  vector 

g„ 

o 

vdot 

=velocity  vector  derivative 

g„ 

o 

vx 

=X  Velocity 

o, 

o 

vy 

=Y  Velocity 

g. 

o 

vz 

=Z  Velocity 

o, 

o 

wb 

=angular  velocity 

g„ 

o 

Wbwi 

=cross  product  matrix 

g. 

o 

wdot 

=angular  velocity  derivative 

o, 

o 

Wewi 

=cross  product  matrix 

g. 

o 

Wq 

=cross  product  matrix 

global  lLong  Re  WzE  Eps2  Gravity  R  e2n  runcount 

%  Variable  Definitions  and  Quaternion  Normalization 

px— u ( 1 ) ; 

py=u ( 2 ) ; 

pz=u  (3) ; 

vx— u ( 4 ) ; 

vy=u ( 5 ) ; 

vz=u (6)  ; 

P  =u ( 7 ) ; 

Q  =u ( 8 ) ; 

R  =u ( 9) ; 

qnorm=norm (u ( 10 : 13 ) ,2)  ; 

q0=u (10) /qnorra; 

ql=u (11)/ qnorra; 

q2=u ( 12 ) / qnorm; 

q3=u ( 13 ) / qnorra; 

t  — u (14); 

dq=u (15) ; 

dr=u (16) ; 

%  Vector  Definitions 
p  = [px;py;pz] ; 
vb= [vx; vy; vz ] ; 
wb=  [ P; Q; R]  ; 
q  =[q0;ql;q2;q3]  ; 

%  Geocentric  Latitude  and  Celestial  Longitude  from  [p] 
latgc=atan (pz/sqrt (pxA2+pyA2 ) )  ; 
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lm  g= (atan2 (py, px) +lLong-WzE*t) ; 
if  lm  g  >  pi 

lm  g=lm  g-pi; 
elseif  lm  g  <  -pi 
lm  g=lm  g+pi ; 

end 

%  Geodetic  Latitude  and  Altitude  about  the  Earth's  Geoid  from  [p] 
h=0;  N=Re;  Nh=N+h;  delta  h=Re; 
while  abs (delta  h)  >  .1 

latgd=atan (pz/sqrt (pxA2+pyA2) / (l-N*Eps2/Nh) ) ; 

N=Re/ sqrt (1-Eps2*sin (latgd)  A2)  ; 

Nh=sqrt (pxA2+pyA2 ) /cos (latgd)  ; 
delta  h=Nh-N-h; 
h=h+delta  h; 

end 

%  Gravity,  Atmosphere  from  EGM1996 
rp=norm (p, 2 ) ; 
egm=diag ( [ 1 ; 1 ; 3 ] ) ; 

g=-Gravity*3.986004418el4/rpA2* (eye (3) +1 . 5*1 . 0826267e-3* (Re/rp) A2*  . 

(egm-5* (pz/rp) A2*eye (3) ) ) *p/rp; 

[ro, press, temp] =STatmos (h) ; 

%  Speed,  Mach  Number,  Angles 
speed=norm (vb, 2 ) ; 

Mspd=sqrt (1 . 402*287*temp) ; 

Mach=speed/Mspd; 

Aa=atan2 (vz ,vx) ; 

As=atan2 (vy, speed)  ; 
if  As<le-3 
As=0 ; 

end 

%  Moment  Matrix,  Aerodynamic  Coefficients 
[ J, Rm, dia, length] =BRParams (t) ; 

CD=ZLDragC (Mach, t) ; 

Sref=pi*diaA2/ 4; 

Drag=ro*speed*vb*CD*Sref / 2  ; 

Qro=0 . 5*ro*speedA2  ; 

[CN_Aa  Cm_Aa  CY_As  Cn_As  Cl_dp  CY_dp  Cn_dp . . . 

Cl  dr  CY  dr  Cn  dr  Cl  As  CN  dq  Cm  dq]=AeroCoeff(Aa,h,dq,t); 

CY=CY_As*As+CY_dr*dr; 

CN=CN_Aa*Aa+CN_dq*dq; 

C1=C1  As*As+Cl  dr*dr+dia/2*speed* [-0 . 5*P] ; 

Cm=Cm  Aa*Aa+Cm  dq*dq+dia/2*speed* [-0 . 5*Q] ; 

Cn=Cn  As*As+Cn  dr*dr+dia/2*speed* [-0 . 5*R] ; 

%  Force,  Torque 
if  t  <  130 

Thrust= [103500*9. 81; 0;0] ; 
elseif  t  <  240 
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Thrust= [2  6200*9 . 81; 0; 0]  ; 

else 

Thrust= [ 0 ; 0 ; 0 ] ; 

end 

Force=Sref *Qro . * [ 0 ; CY; -CN] +Thrust-Drag; 
Torque=Sref *Qro . * [dia*Cl ; 100* length* Cm; dia*Cn] ; 

%  Quaternion  from  ECI  to  Body  Centered 
R_i2b=[q0A2+qlA2-q2A2-q3A2  2* (ql*q2+q0*q3) 

2* (ql*q2-q0*q3)  qO A2 -ql A2+q2 A2 -q3 A2 

2* (ql*q3+q0*q2 )  2* (q2*q3-q0*ql ) 

%  Rotation  from  ECI  to  ECEF  and  from  {b}  to  ECEF 
R  i2e=[  cos (lLong+WzE*t)  sin (lLong+WzE*t)  0; 

-sin (lLong+WzE*t)  cos (lLong+WzE*t)  0; 

0  0  1  ]  ; 

R  b2e=R  i2e*R  i2b'; 

R  n2b=(R  e2n*R  b2e) 

%  Euler  Angles  wrt  {n} 
phi=atan2 (R_n2b (2,3) , R_n2b (3,3) ) ; 
theta=asin ( -R_n2b (1,3)  )  ; 
psy=atan2  (R_n2b  (1,2)  ,  R_n2b  (1,1)  )  ; 

%  Cross-Product  matrices 
Wewi= [  0  -WzE  0; 

WzE  0  0; 

0  0  0  ]  ; 

Wbwi= [ 0  -R  Q; 

R  0  -P; 

-Q  P  0]; 

Wq= [ 0  -P  -Q  -R; 

P  0  R  -Q; 

Q  -R  0  P; 

R  Q  -P  0]; 

%  State  Equations 
pdot=Wewi*p+R  i2b'*vb; 

vdot=R  i2b* (g-WewiA2*p) - (Wbwi+2*R  i2b*Wewi*R  i2b 
wdot=inv(J) *Torque-inv ( J) *Wbwi*J*wb; 
qdot=Wq*q/2 ; 

for  ind=l : 3 

if  wdot ( ind) <le-5 
wdot ( ind) =0 ; 

end 

end 

%  Output  Vector  udot 
udot= [pdot; vdot; wdot; qdot; 

phi; theta;psy; Aa; As; speed; 

latgd; lm_g; h; ] ; 


2* (ql*q3-q0*q2) ; 

2* (q2*q3+q0*ql) ; 
q0A2-qlA2-q2A2+q3A2] 


) *vb+Force/Rm; 
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3. 


SMDetails6.m 


%%  Script  File 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

This  script  defines  mass  and  fuel  parameters  of  the  SM-6  interceptor 
missile,  as  extracted  from  several  open  source  materials  relating  to 
the  Raytheon  SM-6  ERAAW  interceptor  missile  project. 

%  This  script  calculates  the  volume  of  structural  material  and 
%  therefore  weight  of  the  structural  components,  assuming  structural 
%  steel  as  the  primary  structural  material.  The  process  to  determine 
%  the  average  thickness  of  the  structure  was  an  iterative  process  based 
%  on  the  known  information  of  launch  weight,  some  fuel  data,  payload, 

%  and  burn  times. 
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Variables  List 


p_SM_stl_fuel 
p  SM  st2  fuel 
p_SMstr 
r 

rO 
ri 

SM^body 
SM  nose 
SM  stl  fr 
SM_stl_str 
SM  stl  tfm 
SM  st2  fr 
SM_st2_str 
SM  st2  tfm 
th 

V__body 
Venose  str 
V_nose_str0 

Venose  strl 

V_stl_fuel 
V_stl_str 
V_st2  fuel 
V  st2  str 


=length,  varies  by  component 
=density  of  stage  1  rocket  fuel 
=density  of  stage  2  rocket  fuel 
=density  of  structural  material 
=radius,  varies  by  component 
=outer  radius,  varies  by  component 
=inner  radius,  varies  by  component 
=total  mass  of  body  section 
=total  mass  of  nosecone  section 
=fuel  consumption  rate  of  stagel 
=total  mass  of  stage  1  structural  material 
=total  fuel  mass  of  stage  1 
=fuel  consumption  rate  of  stage2 
=total  mass  of  stage  2  structural  material 
=total  fuel  mass  of  stage  2 
=thickness  of  structural  material 
=volume  of  body  structural  material 
=volume  of  nosecone  structural  material 
=volume  of  nosecone  structural  material, 
intermediate  value 

=volume  of  nosecone  structural  material, 
intermediate  value 
=volume  of  stage  1  fuel 

=volume  of  stage  1  structural  material 

=volume  of  stage  2  fuel 

=volume  of  stage  2  structural  material 


global  p  SMstr  p  SM  st2  fuel  p__SM  stl  fuel  SM  nose  SM  st2  str 
global  SM  stl  str  SM  body  SM  st2  tfm  SM  stl  tfm  SM  stl  fr  SM  st2  fr 


%%  Structural  Components 
p_SMstr=7  682  ; 
th=. 007; 


%  Mass  of  Nosecone 
1=. 8255; 
r=0.34/2; 
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V_nose_strO=pi* (1* ( (rA2  +  lA2) / (2*r) ) A2  +  lA3/3- ( ( (rA2  +  lA2)  / (2*r)  )  -r)  * 
(  (rA2  +  lA2) / (2*r) ) A2*asin (1/ ( (rA2  +  lA2) / (2*r) ) ) ) ; 

1=. 8255-th; 
r=0 . 34/2-th; 

V_nose_strl=pi* (1* ( (rA2  +  lA2) / (2*r) ) A2  +  lA3/3- ( ( (rA2  +  lA2) / (2*r) ) -r)  * 

( (rA2+lA2) / (2*r) ) A2*asin (1/ ( (rA2+lA2) /  (2*r)  )  )  )  ; 

V  nose  str=V  nose  strO-V  nose  strl; 

SM  nose=V  nose  str*p  SMstr; 

%  Mass  of  Body/Warhead  Section 

1=3.729-2.88; 

r0=0.34/2; 

ri=0 . 34/2-th; 

V_body=l*pi* (rOA2-riA2) ; 

SM  body=V  body*p_SMstr+115; 

%  Mass  of  Stage  2  (Mkl04  Engine) 

1=2.88 ; 
r0=0.34/2; 
ri=0 . 34/2-th; 

V  st2  str=l*pi* (rO A2-ri A2 ) ; 

SM  st2  str=V  st2  str*p  SMstr; 

%  Mass  of  Stage  1  (Mk72  Booster) 

1=1.72; 
r0=0 . 53/2 ; 
ri=0 . 53/2-th; 

V  stl  str=l*pi* (rO A2-ri A2 ) ; 

SM  stl  str=V  stl  str*p  SMstr; 

%%  Fuel  Components 
%  Stage  2 

V  st2  fuel=0 . 70*l*pi*riA2; 

SM_st2_tfm=360 ; 

p  SM  st2  fuel=SM  st2  tfm/V  st2  fuel; 

SM_st2_fr=3 60/2  0  ; 

%  Stage  1 

V  stl  fuel=0 . 70*l*pi*riA2; 
p_SM_stl_fuel=l 860  ; 

SM  stl  tfm=468; 

SM  stl  fr=468/6; 


102 


o\°  o\°  o\° 


5. 


SMParams6.m 


function  [ J, SM_mass, C] =SMParams6 (t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

This  function  calculates  the  J  Matrix  and  Mass  of  the  interceptor, 
assuming  a  cruciform  rocket  in  two  stages  to  intercept.  This 
function  also  returns  the  reference  (base)  diameter  of  the  missile. 


%%  Notes: 

%  All  CGs  are  measured  from  the  tip  of  the  nosecone. 

%  The  rocket  is  divided  into  five  components:  the  nosecone  and  payload, 
%  stage  one  structure,  stage  one  fuel,  stage  2  structure,  stage  2  fuel. 
%  Solid  fuel  is  assumed  to  burn  from  the  centerline  radially  outward. 

%  A  constant  burn  profile  is  assumed,  such  as  a  star  grain  produces. 

%  See  description  in  SMDetail.m  for  specific  derivation  of  reasons. 

%  The  first  stage  last  6  seconds,  the  second  stage  lasts  an  additional 
%  15  seconds.  Stage  1  (booster)  separates  upon  completion.  Stage  2 
%  does  not  separate  after  completion. 


%  Variable  List 
%  SM_mass 
%  SM_nose 
%  SM_stl_fcr 
%  SM__stl_fuel 

O, 

o 

%  SM_stl_str 
%  SM  stl  tfm 
%  SM  st2  fcr 
%  SM_st2_fuel 

O, 

o 

%  SM_st2_str 
%  SM  st2  tfm 
%  CG 

%  CG_nose 
%  CG_stl 
%  CG_st2 
%  dia 
%  J 
%  Jx 

%  Jx__nose 
%  Jx_stl_fuel 
%  Jx_stl_str 
%  Jx_st2_fuel 
%  Jx_st2_str 
%  Jy 

%  Jy_nose 
%  Jy_stl__fuel 
%  Jy_stl_str 
%  Jy_st2_fuel 
%  Jy_st2_str 
%  Jz 

%  L^SMfuel 
%  p  SMfuel 
%  p_SMstr 


=total  rocket  mass 
=total  mass  of  nosecone  section 
=consumption  rate  of  stage  1  fuel 
=remaining  stage  1  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  1  structural  material 
=total  mass  of  stage  1  fuel 
=consumption  rate  of  stage  2  fuel 
=remaining  stage  2  fuel  based  on  time  and 
consumption  rate 

=total  mass  of  stage  2  structural  material 
=total  mass  of  stage  2  fuel 
=total  rocket  center  of  gravity 
=nosecone  center  of  gravity 
=stage  1  center  of  gravity 
=stage  2  center  of  gravity 
=reference  diameter,  base  diameter 
=Matrix  of  moment  of  inertia 
=total  rocket  moment  of  inertia,  x-axis 
=nosecone  moment  of  inertia,  x-axis 
=stage  1  fuel  moment  of  inertia,  x-axis 
=stage  1  structure  moment  of  inertia,  x-axis 
=stage  2  fuel  moment  of  inertia,  x-axis 
=stage  2  structure  moment  of  inertia,  x-axis 
=total  rocket  moment  of  inertia,  y-axis 
=nosecone  moment  of  inertia,  y-axis 
=stage  1  fuel  moment  of  inertia,  y-axis 
=stage  1  structure  moment  of  inertia,  y-axis 
=stage  2  fuel  moment  of  inertia,  y-axis 
=stage  2  structure  moment  of  inertia,  y-axis 
=total  rocket  moment  of  inertia,  z-axis 
=Length  of  remaining  fuel  in  the  stage 
=density  of  rocket  fuel 
=density  of  structural  material 
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t 


=time 


global  p  SMstr  p_SM  stl  fuel  p_SM  st2  fuel  SM  nose  SM  st2  str 
global  SM  stl  str  SM  body  SM  st2  fuel  mass  SM  stl  fuel  mass 
global  SM  stl  fuel  rate  SM  st2  fuel  rate 

%  Moment  Matrix 
if  t<6 

%  Stage  1  -  Stage  1  Fuel  is  consumed;  Stage  2  Fuel  is  not  used. 

SM  stl  fuel=SM  stl  fuel  mass-SM  stl  fuel  rate*t; 
ri=sqrt (0 . 258A2-SM  stl  fuel/ (p  SM  stl  fuel*l . 72*pi) ) ; 

CG_nose=3/4*0 . 8255*SM_nose; 

CG_body= (0.8255+0.8445/2) *SM_body; 

CG_st2= (1 . 67+2 .88/2) * ( SM_st2_str+SM_st2_fuel__mass ) ; 

CG_stl= (4 . 55+1 .72/2) * ( SM_stl_str+SM_stl_fuel ) ; 

SM  mass=SM  nose+SM  body+SM  st2  str+SM  st2  fuel  mass+SM  stl  str+... 
SM_stl_fuel ; 

CG= (CG_nose+CG_body+CG_st2+CG_stl) /SM_mass; 

Jx  nose=SM  nose*3/10*0 . 17A2; 

Jx_body=SM_body*0 . 17A2/2; 

Jx  st2  str=SM  st2  str* (0 . 17A2+0 . 163A2) /2; 

Jx  st2  fuel=SM  st2  fuel  mass* (0 . 163A2+0 . 091A2) /2; 
Jx_stl_str=SM_stl_str* (0.265A2+0.258A2) /2; 

Jx  stl  fuel=SM  stl  fuel* ( 0 . 258 A2+ . 14 1 A2 ) /2  ; 

Jx=Jx  nose+Jx  body+Jx  st2  str+Jx  st2  fuel+Jx  stl  str+Jx  stl  fuel; 

Jy  nose=SM  nose* ( 12*0 . 17 A2+3*0 . 8255 A2 ) /80+SM  nose* (CG-3*0 . 8255/4 ) A2 ; 
Jy  body=SM  body* (0 . 17A2/4+0 . 8445A2/12) +SM  body*... 

(CG- (078255+0.8445/2) ) A2; 

Jy  st2  str=SM  st2  str* ( (0 . 17A2+0 . 163A2) /4+2 . 88A2/12) +SM  st2  str*... 
(CG- (1 . 67+2 . 88/2) ) A2; 

Jy  st2  fuel=SM  st2  fuel  mass* ( (0 . 163A2  +  0 . 091A2) /4+2 . 88A2/12)  +  .  .  . 

SM_st2_fuel_mass* (CG- (1 . 67+2 . 88/2) ) A2; 

Jy_stl_str=SM_stl_str* ( (0.265A2+0.258A2) /4+2.88A2/12) +SM_stl_str* . . . 
(CG- (4 .55+1 .72/2) ) A2; 

Jy_stl_fuel=SM_st1_fuel* ( (0.258A2+0.141A2) /4+2 . 88 A2 / 12 ) + . . . 

SM_stl_fuel* (CG- (4.55+1.72/2) ) A2; 

Jy=Jy_nose+ Jy_body+ Jy_st2_str+ Jy_st2_fuel+ Jy_stl_str+ Jy_stl_fuel ; 
Jz=Jy; 

dia=0 . 53 ; 

elseif  t<26 

%  Stage  2  -  Stage  1  has  separated;  Stage  2  Fuel  is  consumed. 

SM  st2  fuel=SM  st2  fuel  mass-SM  st2  fuel  rate*t; 
ri=sqrt (0 . 17 A2-SM  st2  fuel/ (p  SM  st2  fuel*2 . 8 8 *pi ) ) ; 

CG_nose=3/4*0 . 8255*SM_nose; 

CG_body= (0.8255+0.8445/2) *SM_body; 

CG_st2  = (1 . 67+2 . 88/2) * ( SM_st2_str+SM_st2_fuel ) ; 

SM  mass=SM  nose+SM  body+SM  st2  str+SM  st2  fuel; 
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CG= (CG_nose+CG_body+CG_st2 ) /SM_mass; 

Jx  nose=SM  nose*3/10*0 . 17A2; 

Jx_body=SM_body*0 . 17A2/2; 

Jx  st2  str=SM  st2  str* (0 . 17A2+0 . 163A2) /2; 

Jx  st2  fuel=SM  st2  fuel* (0 . 163A2+0 . 091A2) /2; 

Jx=Jx  nose+Jx  body+Jx  st2  str+Jx  st2  fuel; 

Jy  nose=SM  nose* ( 12*0 . 17 A2+3*0 . 8255 A2 ) /80+SM  nose*... 

(CG-3*0. 8255/4) A2; 

Jy  body=SM  body* (0 . 17A2/4+0 . 8445A2/12) +SM  body*... 

(CG- (078255+0. 8445/2) ) A2; 

Jy  st2  str=SM  st2  str* ( ( 0 . 17 A2+0 . 1 63 A2 ) /4+2 . 8 8 A2 /12 ) + . . . 

SM~st2_str* (CG- (1 . 67+2 . 88/2) ) A2; 

Jy_st2_fuel=SM__st2_fuel* ( (0 . 163A2+0 . 091A2) /4+2 . 88 A2 / 12 ) + . . . 

SM_st2_fuel* (CG- (1.67+2.88/2)  )  A2; 

Jy=Jy_nose+ Jy_body+ Jy_st2_str+ Jy_st2_fuel ; 

Jz=Jy; 

dia=0 . 34 ; 

else 

%  Stage  3  -  Stage  2  has  not  separated;  entire  missile  is  unpowered 
CG_nose=3/4*0 . 8255*SM_nose; 

CG_body= (0.8255+0.8445/2) *SM_body; 

CG_st2  = (1 . 67+2 . 88/2) *SM_st2_str; 

SM  mass=SM  nose+SM  body+SM  st2  str; 

CG= (CG_nose+CG_body+CG_st2 ) /SM_mass; 

Jx  nose=SM  nose*3/10*0 . 17A2; 

Jx_body=SM_body*0 . 17A2/2; 

Jx  st2  str=SM  st2  str* (0 . 17A2+0 . 163A2) /2; 

Jx=Jx  nose+Jx  body+Jx  st2  str; 

Jy  nose=SM  nose* ( 12*0 . 17 A2+3*0 . 8255 A2 ) /80+SM  nose*... 

(CG-3*0. 8255/4) A2; 

Jy  body=SM  body* (0 . 17A2/4+0 . 8445A2/12) +SM  body*... 

(CG- (078255+0.8445/2) ) A2; 

Jy  st2  str=SM  st2  str* ( (0 . 17A2+0 . 163A2) /4+2 . 88A2/12) +SM  st2  str* 
(CG- (1 . 67+2 . 88/2) ) A2; 

Jy=Jy_nose+ Jy_body+ Jy_st2_str ; 

Jz=Jy; 

dia=0 . 34 ; 

end 

%  Combining  the  results  into  output  variables. 

J= [ Jx  0  0 ; 

0  Jy  0; 

0  0  Jz] ; 


return 
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6.  SMFlight6.m 

function  [udot] =SMFlight6 (u) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  integrates  the  position,  velocity,  and  acceleration 
values 

at  each  time  step  to  determine  the  flight  path  of  a  SM-6  missile 
under  guidance.  The  script  calls  SMParams6.m,  ZLDragC.m,  and 
STatmos .m 


%  Variable 
%  [ACoeff] 
%  Aa 
%  As 
%  Cd 
%  Cl 
%  Cm 
%  CN 
%  Cn 
%  CY 

%  delta_h 
%  dia 
%  dP 
%  dPdot 
%  Drag 
%  dY 
%  dYdot 
%  egm 
%  Force 

%  g 
%  h 
%  J 

%  latgc 
%  latgd 
%  lm  g 
%  Mach 
%  Mspd 
%  N 
%  Nh 
%  P 

%  p 

%  pdot 
%  phi 
%  press 
%  psy 
%  px 

%  py 
%  pz 

%  Q 
%  q 
%  qO 
%  ql 
%  q2 
%  q3 
%  qdot 
%  qnorm 


List : 

=Vector  of  13  coefficients  returned  from  Aerocoeff.m 

=angle  of  attack 

=sideslip  angle 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=aerodynamic  coefficient 

=iterative  value  for  determination  of  altitude 

=reference  diameter,  base  diameter 

=Pitch  control  surface  deflection 

=Pitch  control  surface  deflection  derivative 

=Total  drag  force 

=Yaw  control  surface  deflection 

=Yaw  control  surface  deflection  derivative 

intermediate  value  for  gravity  calculations 

=Total  force  on  the  center  of  mass 

=Gravity 

=Altitude  above  the  Earth  surface 

=Mass  Moment  of  Inertia  Matrix 

=Geocentric  Latitude 

=Geodetic  Latitude 

=Celestial  Longitude 

=speed  in  Mach 

=Local  Speed  of  Sound 

iterative  value  for  determination  of  altitude 
iterative  value  for  determination  of  altitude 
=Roll  Rate 

=Position  vector,  [px;  py;  pz] 

=Position  derivative  vector 
=Euler  Roll  wrt  ECEF 
=atmospheric  pressure 
=Euler  Yaw  wrt  ECEF 
=X  Position 
=Y  Position 
=Z  Position 
=Pitch  Rate 
=quaternion  vector 
=Quaternion ' s  0th  component 
=Quaternion ' s  1st  component 
=Quaternion ' s  2nd  component 
=Quaternion ' s  3rd  component 
=Quaternion  derivative 
=Normalization  of  Quaternion 
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Q. 

O 

Qro 

=Atmospheric  density 

O, 

O 

R 

=Yaw  Rate 

g. 

O 

R  b2e 

=Rotation  Matrix 

Q, 

O 

R  i2b 

=Rotation  Matrix 

g, 

o 

R  i2e 

=Rotation  Matrix 

o, 

o 

R  n2b 

=Rotation  Matrix 

g. 

o 

Rm 

=current  mass 

g. 

o 

ro 

=local  atmospheric  density 

g. 

o 

rp 

=iterative  value  for  gravity 

g„ 

o 

speed 

=velocity  in  m/s 

g. 

o 

t 

=time 

g„ 

o 

theta 

=Euler  Pitch  wrt  ECEF 

g„ 

o 

temp 

=local  atmospheric  temperature 

g. 

o 

Thrust 

=thrust  vector 

g. 

o 

Torque 

=torque  vector 

g. 

o 

u 

=input  vector 

g. 

o 

udot 

=output  vector,  to  be  integrated 

g. 

o 

vb 

=velocity  vector 

g. 

o 

vdot 

=velocity  vector  derivative 

g. 

o 

vx 

=X  Velocity 

g. 

o 

vy 

=Y  Velocity 

g. 

o 

vz 

=Z  Velocity 

g. 

o 

wb 

=angular  velocity 

g. 

o 

Wbwi 

=cross  product  matrix 

g. 

o 

wdot 

=angular  velocity  derivative 

g. 

o 

Wewi 

=cross  product  matrix 

g. 

o 

Wq 

=cross  product  matrix 

global  lLong  Re  WzE  Eps2  Gravity  R  e2n  runcount 

%  Variable  Definitions  and  Quaternion  Normalization 

px— u ( 1 ) ; 

py=u  (2) ; 

pz=u (3) ; 

vx— u  ( 4 ) ; 

vy=u ( 5 ) ; 

vz=u ( 6)  ; 

P  =u  (7)  ; 

Q  =u ( 8 ) ; 

R  =u ( 9 ) ; 

qnorm=norm (u ( 10 : 13 )  ,2)  ; 

q0=u ( 10 ) / qnorra; 

ql=u (11) /qnorra; 

q2=u ( 12 ) / qnorra; 

q3=u ( 13 ) / qnorra; 

t  — u (14); 

dq=u (15) ; 

dr=u (16) ; 

%  Vector  Definitions 
p  = [px;py;pz] ; 
vb= [vx; vy; vz ] ; 
wb=  [ P; Q; R]  ; 
q  =[q0;ql;q2;q3]  ; 

%  Geocentric  Latitude  and  Celestial  Longitude  from  [p] 
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latgc=atan (pz/sqrt (pxA2+pyA2 ) )  ; 
lm  g= (atan2 (py, px) +lLong-WzE*t) ; 
if  lm  g  >  pi 

lm  g=lm  g-pi; 
elseif  lm  g  <  -pi 
lm  g=lm  g+pi; 

end 

%  Geodetic  Latitude  and  Altitude  about  the  Earth's  Geoid  from  [p] 
h=0;  N=Re;  Nh=N+h;  delta  h=Re; 
while  abs (delta  h)  >  .1 

latgd=atan (pz/sqrt (pxA2+pyA2) / (l-N*Eps2/Nh) ) ; 

N=Re/ sqrt (1-Eps2*sin (latgd)  A2)  ; 

Nh=sqrt (pxA2+pyA2 ) /cos (latgd)  ; 
delta  h=Nh-N-h; 
h=h+delta  h; 

end 

%  Gravity,  Atmosphere  from  EGM1996 
rp=norm (p, 2 ) ; 
egm=diag ( [ 1 ; 1 ; 3 ] )  ; 

g=-Gravity*3.986004418el4/rpA2* (eye (3) +1 . 5*1 . 0826267e- . . . 

3* (Re/rp) A2* (egm-5* (pz/rp) A2*eye (3) ) ) *p/rp; 

[ro, press, temp] =STatmos (h) ; 

%  Speed,  Mach  Number,  Angles 
speed=norm (vb, 2 ) ; 

Mspd=sqrt (1 . 402*287*temp) ; 

Mach=speed/Mspd; 

Aa=atan2 (vz,vx) ; 

As=atan2 (vy, speed) ; 
if  As<le-3 
As=0  ; 

end 

%  Moment  Matrix,  Aerodynamic  Coefficients 
[ J, Rm, dia, length] =SMParams6 (t) ; 

CD=ZLDragC (Mach, t) ; 

Sref=pi*diaA2 / 4 ; 

Drag=ro*speed*vb*CD*Sref / 2  ; 

Qro=0 . 5*ro*speedA2  ; 

[CN_Aa  Cm_Aa  CY_As  Cn_As  Cl_dp  CY_dp  Cn_dp . . . 

Cl  dr  CY  dr  Cn  dr  Cl  As  CN  dq  Cm  dq]=AeroCoeff(Aa,h,dq,t); 

CY=CY_As*As+CY_dr*dr; 

CN=CN_Aa*Aa+CN_dq*dq; 

C1=C1  As*As+Cl  dr*dr+dia/2*speed* [-0 . 5*P] ; 

Cm=Cm  Aa*Aa+Cm  dq*dq+dia/2*speed* [-0 . 5*Q] ; 

Cn=Cn  As*As+Cn  dr*dr+dia/2*speed* [-0 . 5*R] ; 

%  Force,  Torque 
if  t  <  130 

Thrust= [206500; 0; 0] ; 
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elseif  t  <  240 

Thrust= [ 95300 ;  0 ;  0 ]  ; 

else 

Thrust= [ 0 ; 0 ; 0 ] ; 

end 

Force=Sref *Qro . * [ 0 ; CY; -CN] +Thrust-Drag; 
Torque=Sref *Qro . * [dia*Cl ; 100* length *Cm; dia*Cn] ; 

%  Quaternion  from  ECI  to  Body  Centered 
R_i2b= [qOA2+ql A2-q2 A2-q3A2  2* (ql*q2+q0*q3) 

2* (ql*q2-q0*q3)  qO A2 -ql A2+q2 A2 -q3 A2 

2* (ql*q3+q0*q2)  2  * (q2 *q3-q0*ql ) 

%  Rotation  from  ECI  to  ECEF  and  from  {b}  to  ECEF 
R  i2e=[  cos (lLong+WzE*t)  sin (lLong+WzE*t)  0; 

-sin (lLong+WzE*t)  cos (lLong+WzE*t)  0; 

0  0  1  ]  ; 

R  b2e=R  i2e*R  i2b'; 

R  n2b=(R  e2n*R  b2e) 

%  Euler  Angles  wrt  {n} 
phi=atan2 (R_n2b (2,3) , R_n2b (3,3) ) ; 
theta=asin ( -R_n2b (1,3)  )  ; 
psy=atan2  (R_n2b  (1,2)  ,  R__n2b  (1,1)  )  ; 

%  Cross-Product  matrices 
Wewi= [  0  -WzE  0; 

WzE  0  0; 

0  0  0  ]  ; 

Wbwi= [  0  -R  Q; 

R  0  -P; 

-Q  P  0]; 

Wq= [  0  -P  -Q  -R; 

P  0  R  -Q; 

Q  -R  0  P; 

R  Q  -P  0]; 

%  State  Equations 
pdot=Wewi*p+R  i2b'*vb; 

vdot=R  i2b* (g-WewiA2*p) - (Wbwi+2*R  i2b*Wewi*R  i2b 
wdot=inv ( J) *Torque-inv ( J) *Wbwi* J*wb; 
qdot=Wq*q/2 ; 

for  ind=l : 3 

if  wdot ( ind) <le-5 
wdot ( ind) =0 ; 

end 

end 

%  Output  Vector  udot 
udot= [pdot;vdot;wdot;qdot; 

phi; theta;psy; Aa; As; speed; 

latgd; lm_g; h; ] ; 


2* (ql*q3-q0*q2) ; 

2* (q2*q3+q0*ql ) ; 
qO A2-ql A2-q2 A2+q3 A2 ] 


) *vb+Force/Rm; 
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7. 


ACoeff.m 


function  [CN  Aa  Cm  Aa  CY  As  Cn  As  Cl  dp  CY  dp  Cn  dp . . . 

Cl  dr  CY  dr  Cn  dr  Cl  As  CN  dq  Cm  dq] =AeroCoef f (Aa, h, dq, t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  interpolates  to  determine  several  aerodynamic 
%  coefficients  to  be  utilized  by  the  BRFlight.m  function  for 
calculation 

%  of  forces  and  moments  acting  on  the  missile. 


%  The  tables  used  were  point-plotted  from  the  book: 

%  Tactical  Missile  Aerodynamics:  General  Topics,  M.J.Hemsch 

(Editor) 

%  Progress  in  Astronautics  and  Aeronautics,  Vol.141,  cl992 

%  Chapter  2,  Aerodynamic  Considerations  for  Autopilot  Design, 

%  L . L . Cronvich,  Figures  8,10,14,17,18,19,23  from  pp. 47-59 


O, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

Q. 

o 

o, 

o 

o, 

o 

o, 

o 

Q. 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 

o, 

o 


Variable  List: 

Aa  =angle  of  attack,  function  input. 

CY  As  =aerodynamic  side  force  coefficient. 

CY  dr  =aerodynamic  yaw  control  force  derivative. 

CY  dp  =aerodynamic  yaw  control  coupling  derivative. 

CN  Aa  =aerodynamic  normal  force  coefficient, 

requires  multiple  interpolations. 

CN  dq  =aerodynamic  pitch  control  force  derivative. 

Cl  As  =aerodynamic  side  moment  coefficient. 

Cl  dr  =aerodynamic  roll  control  coupling  derivative. 

Cl  dp  =aerodynamic  roll  control  moment  derivative. 

Cm  Aa  =aerodynamic  pitching  moment  coefficient, 

requires  multiple  interpolations. 

Cm  dq  =aerodynamic  pitch  control  moment  derivative. 

Cn  As  =aerodynamic  yaw  moment  coefficient. 

Cn  dr  =aerodynamic  yaw  control  moment  derivative. 

Cn  dp  =aerodynamic  yaw  control  coupling  derivative. 

dq  =pitch  controlling  surface  deflection,  utilized  to 

calculate  variables  requiring  multiple  interpolations, 
function  input. 

sign(dq)  =utilized  to  create  symmetric  response  to  dq  commands. 
Obtains  a  value  of  +1  if  dq>0,  -1  if  dq<0. 

h  =altitude  in  meters,  converted  to  km,  function  input. 

nOO, nlO, n20=tabular  value  of  dq  from  point  plots.  Utilized  for 

initial  interpolation  of  CN  Aa,  Cm  Aa,  and  Cl  As  to  create 
the  secondary  interpolation  tables.  The  actual  input 
value  of  dq  is  then  used  to  interpolate  between  the  three 
values . 


AoA  =[  0 

Alt  =[  0 

Tdq  = [  0 


4  8  12 

3.048  6.096  9.144 

10  20  ] ; 


16  20  24  ]  ; 

12.192  15.24  18.288  86 


]  ; 


Aa=Aa* 180/pi ; 
dq=dq*180/pi; 
h=h/1000; 
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if  dq  ==  abs (dq)  %  Assume  the  coefficient  values  are  symmetrical 
sign=l;  %  around  the  mean  line  of  the  control  surface. 

else 

sign=-l ; 

end 


%%  Tables 

%  Fig  8  -  Stability  Data,  Pitch  Plane 


T 

CN 

Aa 

dqn00= [ 

0 

0.545 

1.212 

2.061 

3.212 

4.424 

5.697 

] 

t" 

CN 

Aa 

II 

o 
\ — 1 
C 
tr 
T5 

-0.424 

0.061 

0.727 

1.576 

2.727 

3.879 

5.152 

] 

t" 

CN 

Aa 

dqn20= [ 

-0.969 

-0.485 

0.242 

1 .182 

2.242 

3.455 

4 . 606 

] 

t" 

Cm 

Aa 

dqn00= [ 

0 

-0.394 

-0.848 

-1.394 

-1.969 

-2.545 

-3.212 

] 

t" 

Cm 

Aa 

dqnl0= [ 

2.061 

1 . 697 

1.273 

0.788 

0.303 

-0.182 

-0.667 

] 

T 

Cm 

Aa 

dqn20= [ 

4.545 

3.909 

3.273 

2 . 606 

2 . 121 

1 . 818 

1 . 667 

] 

%  Fig  10  -  Yaw  Stability  Derivatives 

T_CY_As=[  -0.114  -0.118  -0.127  -0.136  -0.152  -0.177  -0.207  ]; 
T_Cn_As=[  0.106  0.085  0.042  -0.015  -0.042  -0.021  0.042  ]; 

%  Fig  14  -  Roll  Derivative 

T_Cl_dp= [  0.08  0.08  0.082  0.084  0.086  0.088  0.09  ]; 


%  Fig  17 

T_CY_dp= [ 
T_Cn_dp= [ 

%  Fig  18 
T_Cl_dr= [ 
T_CY_dr= [ 
T  Cn_dr= [ 


Roll  Control  Coupling  Derivatives 
0  -0.002  -0.005  -0.009  -0.016  -0.024  -0.032  ]; 

0  0.0078  0.019  0.039  0.068  0.104  0.141  ]; 

Yaw  Control  Derivatives 

0  -0.001  -0.004  -0.009  -0.016  -0.023  -0.031  ]; 

0.050  0.050  0.050  0.051  0.053  0.056  0.059  ]; 

-0.209  -0.217  -0.223  -0.231  -0.241  -0.250  -0.266  ]; 


%  Fig  19  -  Effect  of  Pitch  Control  on  Roll  Stability 
T_Cl_As_dqn00= [  0  0.011  0.039  0.106  0.123  0.082  0.021  ]; 

T_Cl_As_dqnlO= [  0  0.033  0.102  0.227  0.269  0.219  0.167  ]; 


%  Fig  23  -  Pitch  Control  Derivatives  for  Aeroelastic  Missile 
T_CN_dq=[  0.1634  0.1649  0.1656  0.1662  0.1665  0.1667  0.1668.. 

0.167  ] ; 

T_Cm_dq= [  0.0689  0.0827  0.0921  0.0985  0.1033  0.1061  0.1076.. 

0.110  ] ; 

%%  Calculation  of  Coefficient  Values 
%  Angle  of  Attack  based  values 

CN  Aa  dqn00=interpl (AoA, T  CN  Aa  dqnOO , Aa, ' spline ') ; 

CN  Aa  dqnl0=interpl (AoA, T  CN  Aa  dqnlO , Aa, ' spline ') ; 

CN  Aa  dqn20=interpl  (AoA,  T  CN  Aa  dqn20,Aa,  'spline'); 

Cm  Aa  dqn00=interpl (AoA, T  Cm  Aa  dqnOO, Aa, 'spline'); 

Cm  Aa  dqnl0=interpl (AoA, T  Cm  Aa  dqnl0,Aa, 'spline'); 

Cm  Aa  dqn20=interpl (AoA, T  Cm  Aa  dqn20,Aa, 'spline'); 

CY  As=interpl (AoA, T  CY  As,Aa, 'spline'); 

Cn  As=interpl (AoA, T  Cn  As,Aa, 'spline'); 

Cl  dp=interpl (AoA, T  Cl  dp,Aa, 'spline'); 

CY  dp=interpl (AoA, T  CY  dp,Aa, 'spline'); 

Cn  dp=interpl (AoA, T  Cn  dp,Aa, 'spline'); 
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Cl  dr=interpl (AoA, T  Cl  dr , Aa, ' spline ') ; 

CY  dr=interpl (AoA, T  CY  dr,Aa, 'spline'); 

Cn  dr=interpl (AoA, T  Cn  dr,Aa, 'spline'); 

Cl  As  dqnOO=interpl (AoA, T  Cl  As  dqnOO,Aa, 'spline'); 
Cl  As  dqnlO=interpl (AoA, T  Cl  As  dqnlO , Aa, ' spline ') ; 

%  Altitude  based  values 

CN  dq=interpl (Alt, T  CN  dq, h, ' linear ') ; 

Cm  dq=interpl (Alt, T  Cm  dq,h, 'linear'); 

%%  Further  Interpolation  of  dq  variables 
T2_CN_Aa=  [  CN_Aa_dqnOO  CN_Aa_dqnlO  CN_Aa__dqn20  ]; 
T2  Cm  Aa= [  Cm  Aa  dqnOO  Cm  Aa  dqnlO  Cm  Aa  dqn20  ]; 

T2  Cl  As= [  Cl  As  dqnOO  Cl  As  dqnlO  Cl  As  dqnlO  ]; 

CN  Aa  l=interpl (Tdq, T2  CN  Aa, abs (dq) ,' spline ') ; 
CN_Aa=CN_Aa_l ; 

Cm  Aa  l=interpl (Tdq, T2  Cm  Aa, abs (dq) ,' spline ') ; 

Cm  Aa=Cm  Aa  1 ; 

Cl  As  l=interpl (Tdq, T2  Cl  As , abs (dq) ,' spline ') ; 
Cl~As=Cl  As  1; 


return 


8.  Animation.m 

%%  Script  File 

%  This  script  'animates'  the  data  received  after  Ballistic  Missile 
Model 

%  run.  It  requires  six  vectors  of  parameters: 

%  speed  -  defining  missile's  full  speed  time  history, 

%  psy, theta, phi  -  defining  Euler  angles  histories  (orientation  of  {b} 
%  wrt  { n } ) , 

%  alpha, beta  -  defining  angle  of  attack  and  sideslip  angle 

%  histories, 

%  E-mail:  oayakime@nps.edu 

clc 

d2r=pi/180; 

Numbof Frs=length (theta) ;  %  Number  of  available  data  points  (frames) 

psyl=pi+psy; 

thetal=theta; 

phil=phi; 

alphal=alpha; 

betal=beta; 

speedl=speed; 

%%  Define  Ballistic  Missile's  geometry  (3  stages) 

%  The  geometry  is  defined  for  a  half  of  the  missile 
xBMs  { 1 }  =  [  0  0  2  3  16  19  30  32]; 
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yBMs { 1 } = [ 0 

1 . 85 

1 . 85 

1.1  1.1 

0.65  0.65 

xBMs { 2 } =  [  0 

0 

1 

2  14 

16]  ; 

yBMs { 2 } = [ 0 

1.31 

1.31 

0.65  0.65 

0]  ; 

xBMs { 3 } =  [  0 

0 

2]  ; 

yBMs { 3 } = [ 0 

0.65 

0]  ; 

%  Define  geometry 

for 

the  second 

half 

for  iMS=l : 3 
nP=length (xBMs { iMS } )  ; 
for  i=l : nP-1 

xBMs { iMS } = [ xBMs { iMS }  xBMs { iMS } (nP-i) ] ; 
yBMs { iMS } = [ yBMs { iMS }  -yBMs { iMS }  (nP-i) ]  ; 
end 

%  Place  the  missile  to  the  center  of  the  image  and  scale  it  so  that  it 
%  occupies  2/3  of  the  screen 
sca=max (xBMs { iMS } ) -min (xBMs { iMS } ) ; 

xbm=3* (xBMs { iMS } -sca/2 ) /sea;  %  Missile  geometry  is  defined  in  NED 
ybm=3*yBMs { iMS } /sea;  %  frame  {b}  (x-axis  is  pointed  North) 

%  The  final  (full,  centered  and  scaled)  geometry 

Missile { iMS }  ( :  ,  1 ) =xbm '  ;  Missile { iMS }  ( : , 2 ) =ones ( length (xbm) , 1 ) ; 

Missile { iMS } ( : , 3 ) =ybm ' ; 
end 


%%  Define  Interceptor's  geometry  (2  stages) 

%  The  geometry  is  defined  for  a  half  of  the  missile 

xIMs { 1 } = [ 0  0  1.6  1.7  1.74  1.83  2  2.1  2.6  2.8  4.5  4.7  6  6.63] 

yIMs { 1 } = [ 0  0.265  0.265  0.17  0.17  0.29  0.33  0.17  0.17  0.3  0.3  0.17  ... 
0.17  0]; 

xIMs { 2 } = [ 0  0  0.04  0.13  0.3  0.4  0.9  1.1  2.8  3  4.3  4.93]; 

yIMs { 2 } = [ 0  0.17  0.17  0.29  0.33  0.17  0.17  0.3  0.3  0.17  0.17  0]; 

%  Define  geometry  for  the  second  half 
for  iIS=l:2 
nP=length (xIMs { ilS } )  ; 
for  i=l : nP-1 

xIMs { ilS }= [xIMs { ilS }  xIMs {i IS] (nP-i) ] ; 
yIMs {iIS}= [yIMs { i I S }  -yIMs{iIS} (nP-i) ] ; 
end 

%  Place  the  missile  to  the  center  of  the  image  and  scale  it  so  that  it 
%  occupies  2/3  of  the  screeng 
sca=max (xIMs { ilS } ) -min (xIMs { ilS } ) ; 

xim=3* (xIMs { ilS } -sca/2 ) /sea;  %  Missile  geometry  is  defined  in  NED 
yim=3*yIMs { ilS } /sea;  %  frame  {b}  (x-axis  is  pointed  North) 

%  The  final  (full,  centered  and  scaled)  geometry 

Interceptor { ilS } ( : , 1) =xim ' ;  Interceptor { ilS } ( : , 2 ) =ones ( length (xim) , 1 ) ; 

Interceptor] ilS } ( : , 3) =yim' ; 

end 


%%  Define  the  initial  frame  for  Missile 
figure ( ' Name ' , ' Side-View  Animation ' ) 


subplot (1,2, 1 ) 

R_psy  =[  cos(psy(l)) 
-sin (psy (1) ) 

0 

R_theta= [  cos (theta ( 1 ) ) 
0 

sin  (theta ( 1 ) ) 
R  phi  = [ 1  0 


sin (psy (1) )  0; 

cos (psy ( 1 ) )  0 ; 

0  1]  ; 

0  -sin (theta (1) ) 

1  0; 

0  cos (theta ( 1 ) ) ] ; 
0; 
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cos (phi ( 1 ) ) 
-sin (phi ( 1 ) ) 


sin (phi (1) ) ; 
cos (phi (1) ) ] ; 


Rm  n2b=R  phi*R  theta*R  psy; 
imMis=Rm  n2b ' *Missile { 1 } ' ; 

Mis=f ill ( imMis (2 ,  : ) , -imMis ( 3 ,  : )  ,  ' c '  ) 
set (Mis , ' EraseMode '  , ' xor ' ) 
axis (2* [-1  1  -1  1]);  axis  equal 
hold  on 
plot  (0,0,  ' r .  ' ) 

plot ( [ 0  0] ,  [0  -.1] ,  'Color' ,  'k' ,  'LineWidth' ,2) 


Rotation  from  {n}  to  {b} 
Missile's  coordinates  in  {n} 
Projection  onto  y-z  plane  of  {n} 


Center  of  the  figure 
Gravity  vector 


sin  (beta ( 1 ) )  0 ; 

cos (beta ( 1 ) )  0 ; 

0  1]  ; 

0  -sin ( -alpha ( 1 ) ) 

1  0; 

0  cos ( -alpha ( 1 ))] ; 

%  Rotation  from  {n}  wrt  {v} 

%  Speed  magnitude  in  {v} 

%  Projection  onto  y-z  plane  of  {n} 
Color '  , ' r ' , ' LineWidth ' ,2) ; 


R_beta  =[  cos (beta (1)) 

-sin  (beta ( 1 ) ) 

0 

R_alpha= [  cos ( -alpha ( 1 ) ) 

0 

sin ( -alpha ( 1 ) ) 

Rm  n2v=R  alpha*Rm  n2b; 

Speed= [ speed ( 1 ) ;  0;  0]/1000; 
imSpd=Rm  n2v'*Speed; 

SpM=plot([0  imSpd(2)],[0  -imSpd(3)], 
set ( SpM, ' EraseMode ' , ' xor ' ) ; 

textSpeed=text ( imSpd (2 ) + . 1 , -imSpd ( 3 ) , ' Speed ' ) ; 
set  (textSpeed,  ' EraseMode ' ,  ' xor ' ) ; 

xlabel ( ' East  (y_{ LTP } ) ' ) ,  ylabel ( ' Up  ( -z_{ LTP } ) ' ) 
set (gca,  ' XTickLabel ',{}),  set (gca,  ' YTickLabel '  ,  { } ) 
title (' Ballistic  Missile  Attitude') 

%%  Display  initial  frame  and  time 

textFrame=text (' Color ', [0 . 8471  0.1608  0 ],' FontAngle ',' italic ' , 
'Position', [1  1.75],' String ' , [ ' Frame  '  num2str ( 1 )  '  out 

num2str (NumbofFrs) ] ) ; 
textTime=text (' Color ' , [0.8471  0.1608 
' Position ',[ 1  1 . 45] ,' String ' , 

'  sec ' ] ) ; 


of 


0] , ' FontAngle ' , ' italic ' , . . . 
'Time  '  num2str (time ( 1 ) , ' % . 2f ' 


sin (psyl ( 1 ) )  0 ; 

cos (psyl ( 1 ) )  0 ; 

0  1]  ; 

0  -sin (thetal ( 1 ) ) 

1  0; 

0  cos  (thetal (1) ) ] ; 

0; 

sin (phil (1) ) ; 
cos (phil (1) ) ] ; 

%  Rotation 


%%  Define  the  initial  frame  for  Interceptor 
subplot (1,2,2) 

R_psy  =[  cos (psyl (1)) 

-sin (psyl (1) ) 

0 

R_theta= [cos (thetal ( 1 )  ) 

0 

sin  (thetal ( 1 ) ) 

R_phi  =[1  0 

0  cos  (phil ( 1 ) ) 

0  -sin (phil ( 1 ) ) 

Ri  n2b=R  phi*R  theta*R  psy; 
imInt=Ri  n2b '* Interceptor { 1 }' ; 

Int=fill (imlnt (2,  : ) , -imlnt (3,  :  )  , 
set  ( Int,  ' EraseMode ' ,  ' xor ' ) ; 
axis (2* [-1  1  -1  1]);  axis  equal 
hold  on 
plot  (0,0,  ' r .  ' ) 

plot  (  [  0  0] ,  [0  -.1] ,  'Color',  'k',  'LineWidth', 2) 
R_beta  =[  cos (betal ( 1 ) )  sin (betal ( 1 ) )  0; 

-sin (betal ( 1 ) )  cos (betal ( 1 ) )  0; 

0  0  1  ]  ; 

R_alpha= [  cos ( -alphal ( 1 ) )  0  -sin ( -alphal ( 1 ) ) 
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'r'  ) 


from  [n]  to  [b] 
Interceptor's  coordinates  in  [n] 
Projection  onto  y-z  plane  of  [n] 


Center  of  the  figure 
Gravity  vector 


0  1  0; 
sin ( -alphal ( 1 ) )  0  cos ( -alphal  ( 1 ) ) ] ; 

Ri  n2v=R  alpha*Ri  n2b;  %  Rotation  from  {n}  wrt  {v} 

Speed= [ speedl ( 1 )  ;  0;  0]/1000;  %  Speed  magnitude  in  {v} 

imSpd=Ri  n2v'*Speed;  %  Projection  onto  y-z  plane  of  {n} 

SpI=plot ( [0  imSpd (2 ) ]  ,  [ 0  -imSpd (3)],  'Color',  'r',  ' LineWidth '  ,  2  )  ; 
set  ( Spl ,  ' EraseMode ' ,  ' xor ' ) ; 

text Speedl =text ( imSpd (2 ) + . 1 , -imSpd ( 3 )  ,  ' Speed ' ) ; 
set (textSpeedl , ' EraseMode ' , ' xor ' ) ; 
xlabel ( ' East  (y_{ DTP } ) ' ) ,  ylabel ( ' Up  ( -z_{ LTP } ) ' ) 
set (gca,  ' XTickLabel ',{}),  set  (gca,  ' YTickLabel ' , { } ) 
title (' Interceptor  Attitude') 


%%  Add  the  'Next  Frame'  and  'Auto'  buttons 
uicontrol ( ' string ' , ' Next  Frame '  ,  ... 

'units', ' normal ','pos',[.66,.15,.13,.06],  ... 

' callback ' ,  ' set (gcf ,  '  ' user data ' ' , 1)  ' )  ; 
auto=uicontrol (' style  '  ,  ' toggle ' ,  'units',  ' norm ' ,  ' pos ' ,  . .  . 

[.8  .15,  .08,  .06],  'string',  'Auto',  ' callback ' ,  ' set (gcf ,  '  .  .  . 
' userdata '  ' , 1 )  '  ) ; 
set (gcf , ' userdata ' , 0 ) ;  goFlag=0 ; 


%%  Start  animation 
for  j =2 : Numbof Frs 

%%  i)  Define  current  stage  for  Missile  and  Interceptor 

if  time (j) <6  %  Interceptor  booster  burns  out 

iMS=l ;  iIS=l; 

elseif  time (j) <130  %  Missile  1st  stage  burns  out 

iMS=l ;  iIS=2 ; 

elseif  time (j) <240  %  Missile  2nd  stage  burns  out 

iMS=2 ;  iIS=2 ; 

else 

IMS =3 ;  iIS=2 ; 

end 


%%  ii)  Define  rotation  matrix  from  {n}  to  { b }  and  rotate  the  missile 
R_psy  =[  cos (psy ( j ) )  sin (psy ( j ) )  0; 

cos (psy ( j ) )  0; 

0  1]  ; 

0  -sin (theta ( j ) ) 

1  0; 

0  cos  (theta ( j ) ) ] ; 

0; 

sin (phi ( j ) ) ; 
cos (phi  ( j ) ) ] ; 

%  Rotation  from  {n}  to  {b} 


-sin (psy ( j ) ) 

0 

R_theta= [cos (theta ( j  )  ) 

0 

sin (theta ( j ) ) 

R  phi  = [ 1  0 

0  cos (phi ( j ) ) 

0  -sin (phi  ( j ) ) 

Rm  n2b=R  phi*R  theta*R  psy; 


imMis=Rm  n2b'*Missile{iMS}'; 

set (Mis , ' XData ' , imMis (2 , : ) , ' YData ' 


%  Missile's  coordinates  in  {n} 
-imMis (3, : ) ) ;  %  y-z  plane  projection 


%%  iii)  Define  rotation 
%%  and  rotate  it 
R__beta  =[  cos(beta(j)) 
-sin  (beta ( j ) ) 
0 

R_alpha= [  cos  ( -alpha ( j ) ) 
0 


matrix  for  the  Missile's 

sin (beta ( j ) )  0 ; 

cos  (beta ( j ) )  0 ; 

0  1]  ; 

0  -sin ( -alpha ( j ) ) 

1  0; 


speed  vector 
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sin ( -alpha ( j ) )  0 


cos (-alpha ( j  )  )  ]  ; 


Rm  n2v=R  alpha*Rm  n2b;  %  Rotation  from  {n}  wrt  {v} 

Speed= [ speed ( j ) ;  0;  0]/1000;  %  Speed  magnitude  in  {v} 

imSpd=Rm  n2v'*Speed;  %  Projection  onto  y-z  plane  of  {n} 

set ( SpM,  ' XData '  ,  [ 0  imSpd (2 ) ] ,  ' YData ' ,  [ 0  -imSpd ( 3 ) ] ) ; 
set (textSpeed, ' Position ',[ imSpd (2 )+. 1  -imSpd(3)  0]); 


%%  iv)  Define  rotation  matrix  from  {n}  to  {b}  and  rotate  the 
interceptor 
R_psy  =[  cos (psyl ( j ) ) 

-sin (psyl ( j ) 

0 

R_theta= [cos (thetal ( j  )  ) 

0 

sin  (thetal ( j ) ) 

R_phi  =[1  0 

0  cos  (phil ( j ) ) 

0  -sin (phil ( j ) 

Ri  n2b=R  phi*R  theta*R  psy; 


sin (psyl ( j ) )  0; 

cos (psyl ( j ) )  0; 

0  1]  ; 

0  -sin (thetal ( j ) ) 

1  0; 

0  cos (thetal ( j ) ) ] ; 

0; 

sin (phil ( j ) ) ; 
cos (phil ( j ) ) ] ; 

%  Rotation 


from  {n}  to  {b} 


imInt=Ri  n2b ' *  Interceptor { ilS } ' ; 

{n} 

set  ( Int,  ' XData ' , imlnt (2 ,  : ) ,  ' YData ' 


%  Interceptor  coordinates  in 
-imlnt (3, : ) ) ;  %  y-z  plane  projection 


%%  v)  Define  rotation  matrix  for  the  Interceptor's 


%%  and  rotate  it 
R_beta  =[  cos (betal ( j ) ) 
-sin (betal ( j ) ) 

0 

R_alpha= [  cos ( -alphal  ( j ) ) 
0 

sin ( -alphal  ( j ) ) 


sin (betal ( j ) )  0 ; 

cos (betal ( j ) )  0 ; 

0  1]  ; 

0  -sin ( -alphal ( j ) ) 

1  0; 

0  cos (-alphal ( j ) ) ] ; 


speed  vector 


Ri  n2v=R  alpha*Ri  n2b;  %  Rotation  from  {n}  wrt  {v} 

Speed= [ speedl ( j ) ;  0;  0]/1000;  %  Speed  magnitude  in  {v} 

imSpd=Ri  n2v'*Speed;  %  Projection  onto  y-z  plane  of  {n} 

set ( Spl , ' XData ' , [ 0  imSpd (2 ) ] , ' YData ' , [ 0  -imSpd ( 3 ) ] ) ; 
set (textSpeedl Position ',[ imSpd (2 )+. 1  -imSpd(3)  0] ) ; 


%%  vi)  Count  frames 

set (textFrame, ' String Frame  '  num2str(j)  '  out  of  '  ... 

num2str (NumbofFrs) ] ) ; 

set (textTime,  ' String '  ,  [ ' Time  '  num2str (time ( j ) ,  ' %  .  2f  '  )  '  sec ' ] ) ; 


%%  vii)  Wait  for  any  control  button  to  be  pushed 
while  goFlag==0 

if  get (auto, 'value ') ==1 
goFlag=l ; 

elseif  get (gcf , ' userdata ' ) ==1 
goFlag=l ; 

set (gcf , ' userdata ', 0 ) 

else 

pause ( . 25) 

end 

end 

goFlag=0 ; 
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pause  (0.1) 
end 


D.  COMMON  PROGRAMS 
1.  ZLDragC.m 

function  [Drag] =ZLDragC (M, t) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  interpolates  to  determine  drag  coefficient  based  on  the 
%  Mach  number  and  the  boost  or  glide  phase  of  the  rocket  to  be  utilized 
%  in  the  BRFlight.m  and  SMFlight.m  programs  for  the  calculation  of 
%  forces  acting  on  the  rocket/missile. 

%  The  tables  used  were  point-plotted  from  Prof  Hutchinson's  ME4703 
%  "Missile  Flight  Analysis"  Class  Notes 

%  Variable  List 

%  BDrag=boost  phase  drag  interpolation  table 
%  GDrag=glide  phase  drag  interpolation  table 
%  M=mach  number 

%  Mach=mach  number  interpolation  table 
%  t=time 


%%  Tables: 


Mach=  [ 

0 

3.5 

0.90 

5.0 

1.1  1.2 
6.0]; 

1.5 

2.0 

2.5 

3.0.  .  . 

BDrag= [ 

0.1444 

0.1185 

0.1444 

0.1000 

0.2778  0.2778 
0.0950] ; 

0.2308 

0.1778 

0.1481 

0.1296 

GDrag= [ 

0.2461 

0.2000 

0.2461 

0.1500 

0.4615  0.4615 
0.1300] ; 

0.3615 

0.2846 

0.2500 

0.2192 

%%  Calculation  of  Drag  Coefficient  Values: 
if  M>6.0 
M=  6 . 0 ; 

end 


BDrag=interpl (Mach, BDrag, M,  'cubic' ) ; 
GDrag=interpl (Mach, GDrag, M, ' cubic ' ) ; ; 

Drag= [BDrag; GDrag] ; 
return 


2.  STatmos.m 

function  [Density,  Pressure,  Temperature] =STatmos (alt) 

%  Calculation  of  the  1976  standard  atmosphere  up  to  86  km 
%  Code  source:  http://www.pdas.com/atmos.htm 

%  Run  ezplot ( ' STatmos (x) ',0,86000)  to  see  the  plot  of  density  vs 
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altitude 


%  Author:  Yakimenko,  Oleg  A. 

%  Date:  September,  27  2005 

%  E-mail:  oayakime@nps.edu 

o, 

o 

alt=alt/ 1 000 ;  %  Convert  altitude  from  m  to  km 

%%  -  Initialize  values  for  1976  atmosphere 

REARTH=63 6 9 . 0 ;  %  Earth  radius  (km),  depends  on  Latitude 

GMR=34 . 163195;  %  Gas  Constant 

htab=[0.0,  11.0,  20.0,  32.0,  47.0,  51.0,  71.0,  84.852];  %  Geometric  alt 
ttab= [288 . 15,  216.65,  216.65,  228.65,  270.65,...  %  Temperature 

270.65,  214.65,  186.946]; 

ptab=[1.0,  2.233611E-1,  5.403295E-2,  8 . 5666784E-3, . . .  %  Relative  pres 

1 . 0945601E-3,  6 . 6063531E-4 ,  3 . 904 6834E-5 ,  3.68501E-6]; 
gtab=[-6.5,  0.0,  1.0,  2.8,  0.0,  -2.8,  -2.0,  0.0];  %  Temp  gradient 

P0=101325 . 0;  Ro0=1.225; 

%% -  Convert  geometric  to  geopotential  altitude 

if  alt>250 
alt=l 00 

end 

h=alt*REARTH/ (alt+REARTH) ; 

%%  -  Binary  search  for  altitude  interval 

i=l  ; 
j=8; 

while  j  >  i+1 

k=f ix ( (i  +  j ) /2)  ; 
if  h<htab ( k) ; 

j=k; 
else 
i=k; 
end 

end 

%%  -  Calculate  local  temperature 

tgrad=gtab  (i) ; 
tbase=ttab ( i ) ; 
deltah=h-htab (i) ; 
tlocal=tbase+tgrad*deltah; 
theta=tlocal/ttab ( 1 ) ; 

%%  -  Calculate  local  pressure 

if  (tgrad  ==  0.0) 

delta=ptab (i) *exp (-GMR*deltah/tbase) ; 
else 

delta=ptab (i) * (tbase/tlocal) A (GMR/tgrad) ; 
end 


%  Isothermal  layers 
%  Non-isothermal  layers 


%%  -  Calculate  local  density 

sigma=delta/ theta; 
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%%  -  Current  atmosphere  parameters  corresponding  to  Altitude  alt 

Temperature=tlocal ;  Density=RoO*sigma;  Pressure=PO*delta; 


return 
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o\°  o\° 


APPENDIX  B:  GUIDANCE  PROGRAMS 


A.  DEMONSTRATION 

%%  Script  File 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  script  generates  the  plots  used  to  demonstrate  the  power  of  the 
%  developed  algorithm  to  change  the  path  and  derivatives  of  the  path 
%  by  varying  the  boundary  conditions  and  virtual  arc,  tau.  The  output 
%  is  two  charts,  with  four  sub-charts  each,  that  show  the  path  for  the 
%  variation  of  the  third  derivative  of  the  initial  condition  and  tau. 


g,  g, 
o  o 

o, 

o 

g, 

o 

g, 

o 

g, 

o 

o, 

o 

o, 

o 

Q. 

o 

o, 

o 

g, 

o 

o, 

o 

o, 

o 

g, 

o 

o, 

o 

g, 

o 

o, 

o 

g, 

o 

g, 

o 

o, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 

g, 

o 


This  script  is  modeled  after  one  developed  by  Oleg  Yakimenko  for  the 
ME4903  course.  Spring  2006 


Variable 

A 

xl 

x2 

tf 

xO 

xpO 

xppO 

xpppO 

xf 

xpf 

xppf 

xpppf 

tau 

N 

al 

a2 

axl 

ax2 

bxl 

bx2 

pxl 

px2 

xl 

x2 

vxl 

vx2 

vl 

v2 

v 


List 

=  matrix  of  reference  equations 
=  first  coordinate  axis 
=  second  coordinate  axis 
=  variable  placeholder  for  tau 

=  initial  position  [xl;x2]  boundary  condition 

=  initial  1st  derivative  of  position  boundary  condition 

=  initial  2nd  derivative  of  position  boundary  condition 

=  initial  3rd  derivative  of  position  boundary  condition 

=  final  position  [xl;x2]  boundary  condition 

=  final  1st  derivative  of  position  boundary  condition 

=  final  2nd  derivative  of  position  boundary  condition 

=  final  3rd  derivative  of  position  boundary  condition 

=  virtual  arc  variable 
=  length  of  tau 

=  xl  boundary  conditions  vector 

=  x2  boundary  conditions  vector 

=  xl  boundary  conditions  reference  equation 

=  x2  boundary  conditions  reference  equation 

=  xl  1st  derivative  of  boundary  conditions  reference 
equation 

=  x2  1st  derivative  of  boundary  conditions  reference 
equation 

=  inversion  of  axl 
=  inversion  of  ax2 
=  xl  reference  trajectory 
=  x2  reference  trajectory 
=  inversion  of  bxl 
=  inversion  of  bx2 

=  xl  1st  derivative  of  reference  trajectory 
=  x2  1st  derivative  of  reference  trajectory 
=  norm  of  [vl;v2] 


syms 

tf 

xO 

xpO  xppO 

xpppO 

xf  xpf 

xppf  xpppf 

real 

A=  [  1 

0 

0 

0 

0 

0 

0 

0; 

0 

1 

0 

0 

0 

0 

0 

0; 

0 

0 

1 

0 

0 

0 

0 

0; 

0 

0 

0 

1 

0 

0 

0 

0; 
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1  tf  tf  A2  / 2  tf A3/6  tf A4/24  tfA5/60  tfA6/120  tfA7/210; 

0  1  tf  tfA2/2  tfA3/6  tf A4/12  tf A5/20  tfA6/30; 

001  tf  tf A2 / 2  tf A3/3  tf A4/4  tfA5/5; 

000  1  tf  tf A2  tf A3  tf A4] ; 

b=[x0  xpO  xppO  xpppO  xf  xpf  xppf  xpppf] 
a=A\b; 

a=collect (a, tf ) 

for  j  =1 : 1 : 4 
xppp0=0 . 3* j -0 . 7 ; 
for  tf=l : 1 : 10; 

al=subs (a, { ' xO ' , ' xpO ' , ' xppO ' , ' xpppO '  , ' xf ' , ' xpf ' , ' xppf ' , ' xpppf '  , 1 tf ' } , 
{ 0 , .2,  .2,  xpppO ,l,0.1,0.1,0.1,tf}) ; 

a2=subs (a,  { ' xO '  ,  ' xpO ' ,  ' xppO ' ,  ' xpppO '  ,  ' xf  '  ,  ' xpf ' ,  ' xppf ' ,  ' xpppf '  ,  ' tf ' } , 

{0,l,0.1,0.1,l,-l,0.1,0.1,tf}) ; 
axl=diag (  [1, 1, 1/2, 1/6,  1/2  4, 1/60, 1/120, 1/210] ) *al; 
ax2=diag ( [1, 1, 1/2, 1/6, 1/24, 1/60, 1/120, 1/210] ) *a2; 
bxl=diag ( [0, 1, 1, 1/2, 1/6, 1/12, 1/20, 1/30] ) *al; 
bx2=diag ( [0, 1, 1, 1/2, 1/6, 1/12, 1/20, 1/30] ) *a2; 
tau= [ 0 : . 05 : tf ] ; 

N=length (a) ; 

figure  ( 1 ) 
subplot  (2 , 2 , j ) 
for  i=l : N 

pxl (i) =axl (N+l-i) ; 
px2 (i) =ax2 (N+l-i) ; 

end 

xl=polyval (pxl , tau) ; 
x2=polyval (px2 , tau) ; 
line(xl,x2, ' Linewidth ' , 2 ) 
axis ( [0  2  0  6] ) 
xlabel ( ' x  1 ' ) ,  ylabel ( ' x  2 ' ) 
text (1.1,2, ' \tau_f=var ' ) 

title ( [ 'dA3x/dtA3 | _ 0= '  num2str (xpppO ) ] ) 

grid  on; 

figure  (2 ) 
subplot  (2 , 2 , j ) 
for  i=l : N-l 

vxl (i) =bxl (N+l-i) ; 
vx2 (i) =bx2 (N+l-i) ; 

end 

vl=polyval (vxl , tau) ; 
v2=polyval (vx 2 , tau) ; 
v=sqrt (vl . A2+v2 . A2 ) ; 
line (tau, v, ' Linewidth ', 2 ) 
axis (  [0  10  0  4] ) 

xlabel ( ' \tau ' ) ,  ylabel ( ' \surd (x_l A2+x_2  A2 )  ' ) 
text (1.1,2,  ' \tau_f=var ' ) 

title ( [ 'dA3x/dtA3 | _ 0= '  num2str (xpppO ) ] ) 

grid  on 
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end 

end 


B.  GUIDANCE  ALGORITHMS 
1.  SMGuidance.m 

function  [path] =SMGuidance (time, state,  target,  i)  ; 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  takes  in  the  state  of  the  interceptor  and  target  and 
%  generates  an  initial  guess  at  the  final  conditions  (position, 

%  orientation  angles,  range,  and  time  to  intercept)  through  a  first- 
order 

%  trajectory  assumption  and  iterative  process.  It  then  calls  the 
%  fminsearch  function  using  those  initial  guesses.  Finally,  it  plots 
the 

%  returned  optimal  flight  path  and  associated  variables. 


%%  Variable 
%  best 

o, 

o 

%  const 

O, 

o 

%  cost 
%  costs 
iteration 
%  delta 

o, 

o 

%  free 

o, 

o 

%  init 
%  J 
%  N 

%  nmax 

O, 

o 

%  options 

O, 

o 

%  path 
%  psi 
%  psidot 
%  psif 

O, 

o 

%  psit 
%  Py 
%  Pz 
%  q 
%  qq 
%  range 
%  state 

Q, 

O 

%  states 
%  target 

o, 

o 


List 

=  vector  of  the  variables  in  the  optimal  path  returned  from 
the  fminsearch  function 

=  variables  that  fminsearch  cannot  modify,  including  system 
constraints 

=  cost  function  value  returned  from  SMGuidanceCost .m 
=  array  of  the  value  of  the  cost  variables  at  each 

=  difference  between  the  tgol  and  tgo2  values  (used  in  the 
iteration  of  initial  guesses) 

=  variables  that  fminsearch  can  modify,  specifically 
[tau; tgo; thf ;psif ] 

=  vector  of  initial  estimates 
=  vector  of  cost  function  variable  values 
=  length  of  the  path  vector  (used  for  plotting) 

=  maximum  acceleration  capability  of  the  interceptor, 
altitude  dependent 

=  modifiable  options  for  the  fminseach  function  (MATLAB 
help  file  is  under  "optimset") 

=  returned  time  history  of  the  optimal  path 
=  initial  interceptor  heading  angle 

=  initial  rate  of  change  of  interceptor  heading  angle 
=  final  interceptor  heading  angle,  calculated  from  final 
conditions  estimate 
=  target  heading  angle 

=  penalty  function  on  the  y  acceleration 
=  penalty  function  on  the  z  acceleration 
=  counting  variable 
=  counting  variable 

=  estimate  of  distance  between  target  and  interceptor 
=  state  of  the  interceptor  missile,  sent  from  SMGuidance.m 
[px;py;pz; V; th;psi; Vdot; thetadot;psidot] ; 

=  array  of  the  values  of  all  processes  in  SMGuidance.m 
=  state  of  the  rocket,  sent  from  SMGuidance.m 
[Pos_BR (i+j , 1) ; Pos_BR (i+ j , 2) ; Pos_BR (i+j  ,  3 )  ; 
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o, 

o 

o, 

o 

o, 

o 

%  tau_f 
%  tgo 
%  tgol 
%  tgo2 
%  th 
%  thdot 
%  thf 
%  tht 

%  tic . . toe 
%  trys 
%  V 
%  V_f 
%  Vave 
%  Vdot 
%  xO 
%  xdO 
%  xddO 
%  xdf 
%  xdt 
%  xf 
%  xmult 
%  xt 
%  ymult 
%  zmult 


Vel  BR (i+j , 1) ; Vel  BR ( i+ j , 2 ) ; Vel_BR ( i+ j , 3 ) ]  where  ' i+j ' 
is  the  time  synchronization  function  between  BRFlight.m 
and  SMFlight.m 
=  value  of  the  virtual  arc 
=  time  to  go  to  intercept 

=  estimate  of  time  to  go  to  intercept,  iterative  value 
=  estimate  of  time  to  go  to  intercept,  iterative  value 
=  initial  interceptor  flight  path  angle 

=  initial  rate  of  change  of  interceptor  flight  path  angle 
=  final  interceptor  flight  path  angle 
=  target  flight  path  angle 
=  MATLAB  function  to  track  run  time 
=  vector  of  optimal  path  and  derivative  values 
=  initial  interceptor  velocity 
=  final  interceptor  velocity 
=  average  interceptor  velocity 
=  initial  interceptor  acceleration 
=  initial  interceptor  position 
=  initial  interceptor  velocity 
=  initial  interceptor  acceleration 
=  final  interceptor  position 
=  current  target  velocity 
=  final  interceptor  acceleration 
=  ratio  value  (used  for  plotting) 

=  current  target  position 
=  ratio  value  (used  for  plotting) 

=  ratio  value  (used  for  plotting) 


global  costs  q  qq  states  tgo  trys  update 

%  Initialize  Variables 
Re= 6 .378137e6; 
q=q+l; 

%  SM  Initial  State 
xO=state (1:3)  ; 

V=state ( 4 ) ; 
th=state  ( 5 ) ; 
psi=state ( 6) ; 

Vdot=state  ( 7 )  ; 
thdot=state  (8) ; 
psidot=state (9) ; 

%  SM  Initial  Velocities  and  Accelerations 
xdO= [V*cos (th) *cos (psi) ; 

V*cos (th) *sin (psi) ; 

V*sin (th) ] ; 

xddO= [Vdot* cos (th) *cos (psi) -V*cos (th) *sin(psi) *psidot-V*sin (th) * . . . 
cos (psi) *thdot; 

Vdot*cos (th) *sin (psi) +V*cos (th) *cos (psi) *psidot-V*sin (th) * . . . 
sin (psi) *thdot; 

Vdot*sin (th) +V*cos (th) *thdot]  ; 


%  BR  Initial  State 
xt=target (1:3) ; 
xdt=target (4:6) ; 

tht=atan2 (xdt ( 3 ) , norm (xdt (1:2))); 
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psit=atan2 (xdt (2 ) , xdt ( 1 ) ) ; 


%  Estimate  Final  Conditions 
tgol=100;  delta=5; 
while  delta>l 
if  tgol>26 

V_f=3500-10* (tgol-26) ; 

Vave= (26*3500/2+ (tgol-26) * (3500+V_f ) /2) /tgol; 

else 

V_f=tgol*3500/26; 

Vave=V  f/2; 

end 

xf=xt+xdt . *tgol ; 

tgo2=sqrt ( (xf ( 1 ) -xO ( 1 ) ) A2+ (xf (2) -xO (2) ) A2+ (xf (3) -xO (3) ) A2) . . . 

/ (norm (xdt) +Vave) ; 
delta=abs (tgo2-tgol) ; 
tgol= (tgol+tgo2 ) /2 ; 

end 

tgo=tgol ; 

range=sqrt ( (xf (1) -xO (1) ) A2+ ( (xf (2) -xO (2) ) ) A2+ ( (xf (3) -xO (3) ) ) A2) 

tau_f=30-10*q; 

thf=-tht; 

psif=psit+pi ; 

init= [tau_f ; tgo; thf ;psif ] ; 

%  Collect  Constraints 
free=init; 

const= [xO ; xdO ; xddO ; time; xt ; xdt;  init ]  ; 

%  Search  for  Minimum  Cost  function 
tic 

options=optimset ( ' Max I ter ' , 100, 'Tolfun' , 10, 'TolX' , 10) ; 

best  =  fminsearch ( @ (x)  SMGuidanceCost (x, const) , free, options) ; 

toe 

[path] =SMTra j ectory (best,  const)  ; 

[cost,  J,  Py,  Pz] =SMGuidanceCost (best, const) 

tau_f=best (1) ; 
tgo=best (2 ) ; 
thf=best (3); 
psif=best ( 4 ) ; 

N=length (path ( : , 1 ) ) ; 

V_f=path (N, 5 ) 
xf=path (N, 2 : 4 ) 

xdf= [V_f *cos (thf) *cos (psif ) ; 

V_f *cos (thf) *sin (psif) ; 

V_f *sin (thf) ]  ; 

%%  Plot  Every  Iteration 
xmult=20000/ (norm (xdO) +norm (xdt) ) ; 
ymult=20000/ (norm (xdO) +norm (xdt) ) ; 
zmult=20000/ (norm (xdO) +norm (xdt) ) ; 
nmax=4  0+ (40-10) / (0-50000) * (path ( : ,  4) -Re)  ; 
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figure 

plot 3 (path ( : , 2 ) , path ( : , 3 ) , path ( : , 4 ) , ' -k ' , ' Linewidth ' , 3 ) 
hold  on; grid; 

plot 3 (xO (1) , xO (2) ,x0 (3) , '  *b ' , ' linewidth ' , 5 ) 
plot3 ( [xO (1) -xmult*xdO (1) ;x0 (1) +xmult*xdO (1) ]  ,  .  .  . 

[xO (2 ) -ymult*xdO (2 ) ; xO (2 ) +ymult*xdO (2 ) ]  ,  .  .  . 

[xO ( 3 ) -zmult*xdO ( 3 ) ; xO ( 3 ) +zmult*xdO ( 3 ) ]  ,  ' -b ' ,  ' Linewidth ' , 3 ) 
plot 3 (xO (1) +xmult*xdO (1) , xO (2) +xmult*xdO (2) , xO (3) +xmult*xdO (3) , . . . 
' ob ' , ' linewidth ' , 3 ) 

plot 3 (xf (1) , xf (2) ,xf (3) , ' *r ' , ' linewidth ' , 5 ) 
plot 3 ( [xf ( 1 ) -xmult*xdt ( 1 ) ; xf ( 1 ) +xmult*xdt ( 1 ) ] , . . . 

[xf (2 ) -ymult*xdt (2 ) ; xf (2 ) +ymult*xdt (2 ) ]  ,  .  .  . 

[xf ( 3 ) -zmult*xdt ( 3 ) ; xf ( 3 ) +zmult*xdt ( 3 ) ] , ' -r ' , ' Linewidth ' , 3 ) 
plot 3 (xf (1) +xmult*xdt (1) , xf (2) +xmult*xdt (2) , xf (3) +xmult*xdt (3) , . . . 
' or ' , ' linewidth ' , 3 ) 

%axis ( [ 0  250000  -150000  200000  6300000  6550000]) 

xlabel('X  position  (m) FontName Times  New  Roman') 

ylabel('Y  position  (m) FontName Times  New  Roman') 

zlabel ( ' Z  position  (m) ',' FontName ',' Times  New  Roman') 

title  (  ' Interception  Geometery ' ,  ' FontName ' ,  ' Times  New  Roman ' ,  .  .  . 

' Fontsize ',12.5) 


figure 

plot 3 (path ( : , 2 ) , path ( : , 3 ) , path ( : ,  4 ) -Re,  ' -k ' ,  ' Linewidth ' , 3 ) 
hold  on ; grid; 

plot 3  (xO (1) ,x0 (2) ,x0 (3) -Re,  ' *b ' ,  ' linewidth ' , 5 ) 
plot3 ( [xO (1) -xmult*xd0 (1) ;x0 (1) +xmult*xd0 (1) ]  ,  .  .  . 

[xO (2 ) -ymult*xd0 (2 ) ; xO (2 ) +ymult*xd0 (2 ) ]  ,  .  .  . 

[xO ( 3 ) -Re-zmult*xd0 ( 3 ) ; xO ( 3 ) -Re+zmult*xd0 ( 3 ) ] , ' -b ' , ' Linewidth ' , 3 ) 
plot 3 (xO (1) +xmult*xd0 (1) , xO (2) +xmult*xd0 (2) , xO (3) -Re+xmult*xd0 (3) , . . . 

' ob ' , ' linewidth ' , 3 ) 

plot 3 (xf (1) , xf (2) ,xf (3) -Re, ' *r ' , ' linewidth ' , 5 ) 
plot3 ( [xf (1) -xmult*xdt (1) ;xf (1) +xmult*xdt (1) ] , . . . 

[xf (2 ) -ymult*xdt (2 ) ; xf (2 ) +ymult*xdt (2 ) ]  ,  .  .  . 

[xf ( 3 ) -Re-zmult*xdt ( 3 ) ; xf ( 3 ) -Re+zmult*xdt ( 3 ) ] , ' -r ' , ' Linewidth ' , 3 ) 
plot3 (xf (1) +xmult*xdt (1) , xf (2) +xmult*xdt (2) , xf (3) -Re+xmult*xdt (3) , ... 

' or ' , ' linewidth ' , 3 ) 

%axis ( [ 0  250000  -150000  200000  6300000  6550000]) 
xlabel('X  position  (m) ',' FontName ',' Times  New  Roman') 
ylabel('Y  position  (m) ',' FontName ',' Times  New  Roman') 
zlabel (' Z  position  (m) ',' FontName ',' Times  New  Roman') 

figure 

plot (path ( : , 2 ) , path (:,3), ' -k ' , ' Linewidth ' , 3 ) 

hold  on; grid; %axis  equal; 

plot  (xO ( 1 ) , xO (2 ) ,  ' *b ' ,  ' linewidth ' , 5 ) 

plot ( [xO (1) -xmult*xd0 (1) ;x0 (1) +xmult*xd0 (1) ]  ,  . . . 

[xO (2 ) -ymult*xd0 (2 ) ; xO (2 ) +ymult*xd0 (2 ) ] , ' -b ' , ' Linewidth ' , 3 ) 
plot (xO ( 1 ) +xmult*xd0 ( 1 ) , xO (2 ) +xmult*xd0 (2 ) , ' ob ' , ' linewidth ' , 3 ) 
plot (xf ( 1 ) , xf (2 ) , ' *r ' , ' linewidth ' , 5 ) 
plot ( [xf ( 1 ) -xmult*xdt ( 1 ) ; xf ( 1 ) +xmult*xdt ( 1 ) ] , ... 

[xf (2 ) -ymult*xdt (2 ) ; xf (2 ) +ymult*xdt (2 ) ] , ' -r ' , ' Linewidth ' , 3 ) 
plot (xf ( 1 ) +xmult*xdt ( 1 ) , xf (2 ) +xmult*xdt (2 ) ,  ' or '  ,  ' linewidth ' , 3 ) 
xlabel('X  position  (m) ',' FontName ',' Times  New  Roman') 
ylabel('Y  position  (m) ',' FontName ',' Times  New  Roman') 
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figure 

plot (path ( : , 3 ) , path ( : , 4 ) -Re, ' -k ' , ' Linewidth ' , 3 ) 

hold  on; grid; %axis  equal; 

plot(xO(2),xO(3)-Re, '  *b '  , ' linewidth ' , 5 ) 

plot ( [xO (2) -ymult*xdO (2) ;x0 (2) +ymult*xdO (2) ]  ,  . . . 

[xO ( 3 ) -Re-zmult*xdO ( 3 ) ; xO ( 3 ) -Re+zmult*xdO ( 3 ) ] , ' -b ' , ' Linewidth ' , 3 ) 
plot (xO (2 ) +xmult*xdO (2 ) , xO ( 3 ) -Re+xmult*xdO ( 3 ) , ' ob ' , ' linewidth ' , 3 ) 
plot(xf(2),xf(3)-Re, ' *r ' , ' linewidth ' , 5 ) 
plot ( [xf (2) -ymult*xdt (2) ;xf (2) +ymult*xdt (2) ] , . . . 

[xf ( 3 ) -Re-zmult*xdt ( 3 ) ; xf ( 3 ) -Re+zmult*xdt ( 3 ) ] ,  ' -r '  ,  ' Linewidth ' , 3 ) 
plot (xf (2 ) +xmult*xdt (2 ) , xf ( 3 ) -Re+xmult*xdt ( 3 ) ,  ' or  '  ,  ' linewidth ' , 3 ) 
xlabel('Y  position  (m) FontName Times  New  Roman') 
ylabel ( ' Z  position  (m) FontName Times  New  Roman') 

figure 

plot (path ( : , 2 ) , path ( : , 4 ) -Re,  ' -k '  ,  ' Linewidth ' , 3 ) 

hold  on; grid; %axis  equal; 

plot(xO(l),xO(3)-Re, ' *b ' , ' linewidth ' , 5 ) 

plot ( [xO ( 1 ) -xmult*xdO (1 ) ; xO (1 ) +xmult*xdO ( 1 ) ]  ,  ... 

[xO ( 3 ) -Re-zmult*xdO ( 3 ) ; xO ( 3 ) -Re+zmult*xdO ( 3 ) ] , ' — b ' , ' Linewidth ' , 3 ) 
plot (xO ( 1 ) +xmult*xdO ( 1 ) , xO ( 3 ) -Re+xmult*xdO ( 3 ) , ' ob ' , ' linewidth ' , 3 ) 
plot(xf(l),xf(3)-Re, ' *r ' , ' linewidth ' , 5 ) 
plot ( [xf (1) -xmult*xdt (1) ;xf (1) +xmult*xdt (1) ] , . . . 

[xf ( 3 ) -Re-zmult*xdt ( 3 ) ; xf ( 3 ) -Re+zmult*xdt ( 3 ) ] , ' -r ' , ' Linewidth ' , 3 ) 
plot (xf ( 1 ) +xmult*xdt ( 1 ) , xf ( 3 ) -Re+xmult*xdt ( 3 )  ,  ' or ' ,  ' linewidth ' , 3 ) 
xlabel('X  position  (m) ',' FontName ',' Times  New  Roman') 
ylabel (' Z  position  (m) ',' FontName ',' Times  New  Roman') 

figure 
hold  on 

plot  (path ( : , 1 ) , path ( : , 5 )  ,  ' -b ' ,  ' Linewidth ' , 3 ) 
plot  (path ( : , 1 ) , path ( : , 8 ) ,  ' —  k ' ,  ' Linewidth ' , 3 ) 
grid 

xlabel ( ' time  ( s ) ' , ' FontName ' , ' Times  New  Roman ' ) 
ylabel ( ' V ' ,  ' FontName '  ,  ' Times  New  Roman ' ) 
legend ( ' V ' , ' Vdot ' , 'Location ' , ' Ea stOut side ' ) 

figure 
hold  on 

plot (path ( : , 1 ) , path ( : , 6) , ' -b ' , ' Linewidth ' , 3 ) 
plot (path ( : , 1 ) , path ( : , 7 ) ,  ' —  k ' ,  ' Linewidth ' , 3 ) 
grid 

xlabel ( ' time  ( s ) ' , ' FontName ' , ' Times  New  Roman ' ) 
ylabel ('th,  psi  ( rad) ',' FontName ',' Times  New  Roman') 
legend ( ' th ' , ' psi ' , 'Location ' , ' EastOutside ' ) 

figure 
hold  on 

plot (path ( : , 1 ) , path ( : , 12 ) , ' --b ' , ' Linewidth ' , 3 ) 
plot  (path ( : , 1 ) , path ( : , 1 3 ) ,  ' -k ' ,  ' Linewidth ' , 3 ) 
plot (path ( : , 1 ) , nmax ( : ) , ' --r ' , ' Linewidth ' , 3 ) 
plot (path ( : , 1 ) , -nmax ( : ) , ' --r ' , ' Linewidth ' , 3 ) 
grid 

xlabel ( ' time  ( s ) ' , ' FontName ' , ' Times  New  Roman ' ) 
ylabel ( ' force  (g) ' , ' FontName ' , ' Times  New  Roman ' ) 
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legend ('ny', ' nz ' , 'Location', ' Ea stOut side ' ) 


figure 

subplot (3,1,1) 

plot (path ( : , 1 ) , trys (:,1), ' -k ' , ' Linewidth ' , 2 ) ; 

legend ( ' x ( 1 ) ' ) 

grid 

subplot ( 3 , 1,2) 

plot (path ( : , 1 ) , trys ( : , 5 ) , ' -k ' , ' Linewidth ' , 2 ) ; 
ylabel (' Flight  Path ',' FontName ',' Times  New  Roman') 
legend ( ' xA / ( 1 ) ' ) 
grid 

subplot (3,1,3) 

plot  (path ( : , 1 ) , trys ( : , 9) ,  ' -k ' ,  ' Linewidth '  ,  2  )  ; 

legend (' xA/A/ (1)  ') 

grid 

xlabel ( ' time ' , ' FontName ' , ' Times  New  Roman ' ) 
figure 

subplot  ( 3 , 1 , 1 ) 

plot (path ( : , 1 ) , trys ( : , 2 ) , ' -k ' , ' Linewidth ' , 2 ) ; 
grid 

legend ( ' x (2 ) ' ) 
subplot (3,1,2) 

plot  (path ( : , 1 ) , trys ( : , 6) ,  ' -k ' ,  ' Linewidth ' , 2 ) ; 

ylabel ('1st  Dirivative  of  Flight  Path ',' FontName ',' Times  New  Roman') 

legend ( ' xA /  (2 )  '  ) 

grid 

subplot (3,1,3) 

plot (path ( : , 1 ) , trys ( :  ,  10 )  ,  '  k  '  ,  ' Linewidth ' , 2 ) ; 

legend ( ' xA / A / (2) ' ) 

grid 

figure 

subplot (3,1,1) 

plot (path ( : , 1 ) , trys (:,3), ' -k ' , ' Linewidth ' , 2 ) ; 

legend ( ' x ( 3 ) ' ) 

grid 

subplot ( 3 , 1,2) 

plot (path ( : , 1 ) , trys (:,7), ' -k ' , ' Linewidth ' , 2 ) ; 

ylabel ('2nd  Derivative  of  Flight  Path ',' FontName ',' Times  New  Roman') 

legend ( 'xA/ (3) ' ) 

grid 

subplot (3,1,3) 

plot (path ( : , 1 ) , trys ( : , 1 1 ) , ' -k ' , ' Linewidth '  ,  2  )  ; 

legend (' xA/A/ (3)  ') 

grid 

xlabel ( ' time  (s) ' , ' FontName ' , ' Times  New  Roman ' ) 
figure 

plot(costs(:,l), ' -k ' , ' Linewidth ' , 3) ; grid 

xlabel ( ' Iterations  (#)  '  ,  ' FontName ' ,  ' Times  New  Roman ' ) 

ylabel ('Cost  Function  Value ',' FontName ',' Times  New  Roman') 

figure 

plot(costs(:,2), ' -k ' , ' Linewidth ' , 3) ; grid 
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xlabel (' Iterations  (#)',' FontName Times  New  Roman') 
ylabel ( ' / tau  f ' , ' FontName ' , ' Times  New  Roman ' ) 


figure 

plot(costs(:,3), ' -k ' , ' Linewidth ' , 3) ; grid 

xlabel (' Iterations  (#)',' FontName Times  New  Roman') 

ylabel ('t  g  o  to  Intercept ',' FontName ',' Times  New  Roman') 


figure; hold  on; 

plot(costs(:,5), ' -b ' , ' Linewidth ' , 3) 
grid 

plot(costs(:,6), ' -k ' , ' Linewidth ' , 3 ) 
hold  off 

xlabel (' Iterations  (#)',' FontName ',' Times  New  Roman') 
ylabel (' Penalty  Values ',' FontName ',' Times  New  Roman') 
legend ( ' Py ' , ' Pz ' ) 


figure 

pi ot  (180/pi* acos (costs (:, 4) /1 00 ) ,  ' -k ' ,  ' Linewidth ' , 3 ) ; grid 
xlabel (' Iterations  (#)',' FontName ',' Times  New  Roman') 
ylabel ( ' Impact  Angle ' , ' FontName ' , ' Times  New  Roman ' ) 
axis ( [ 0  qq  0  100 ] ) 

o,  o, 
o  o 

states { q, 1 } =path; 

states [q, 2 }=const; 

states {q, 3 }=free; 

states {q, 4 }=best; 

states {q,  5 }= [cost; J; Py; Pz] ; 


return 


2.  SMGuidanceCost.m 

function  [cost, J, Py, Pz] =SMGuidanceCost (free,  const) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  calculates  the  cost  of  the  proposed  trajectory  returned 
%  from  the  SMTrajectory.m  function  based  on  the  optimization  parameters 
%  and  penalty  parameters  defined  herein.  This  is  a  sub-function  of  the 
%  SMGuidance.m  function's  fminsearch.  This  cost  value  is  used  to 
%  determine  whether  the  proposed  trajectory  is  optimal.  The  trajectory 
%  that  returns  the  minimum  value  of  J  is  the  optimal  function. 


%%  Variable 
%  calccost 

o, 

o 

%  const 

o, 

o 

%  cost 
function 
%  costs 
%  dist 
%  free 


List 

=  a  global  variable  of  the  value  of  the  J  function  (used 
for  plotting) 

=  variables  that  fminsearch  cannot  modify,  including  system 
constraints,  specifically  [xO ; xdO ; xddO ; time; xt ; xdt ; init ] 

=  cost  function  value  returned  from  SMGuidanceCost.m 

=  vector  of  values  of  the  cost  variables  at  each  iteration 
=  cumulative  distance  traveled 

=  variables  that  fminsearch  can  modify,  specifically 
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Q, 

O 

%  init 
%  J 
%  N 

%  nmax 

O, 

o 

%  nx 
%  ny 
%  nz 
%  path 

Q. 

O 

nz  '  ] 

%  psi 
%  psidot 
%  psif 

O, 

o 

%  Py 
%  Pz 

%  qq 
%  t 

%  tau_f 
%  tgo; 

%  th 
%  thdot 
%  thf 
%  time 
%  V 
%  V_f 
%  Vdot 
%  X 
%  xO 
%  xdO 
%  xddO 
%  xdf 
%  xdt 
%  xt 


[tau; tgo; thf ; psif ] 

=  vector  of  initial  estimates 
=  vector  of  cost  function  variable  values 
=  length  of  the  path  vector  (used  for  plotting) 

=  maximum  acceleration  capability  of  the  interceptor, 
altitude  dependent 

=  axial  acceleration  command,  body  frame  x 
=  axial  acceleration  command,  body  frame  y 
=  axial  acceleration  command,  body  frame  z 
=  returned  time  history  of  the  optimal  path,  specifically 
[time'  X(l:3,:)'  V'  th '  psi'  Vdot'  thdot'  psidot'  nx '  ny' 

=  initial  interceptor  heading  angle 

=  initial  rate  of  change  of  interceptor  heading  angle 
=  final  interceptor  heading  angle,  calculated  from  final 
conditions  estimate 

=  penalty  function  on  the  y  acceleration 

=  penalty  function  on  the  z  acceleration 

=  counting  variable 

=  current  time 

=  value  of  the  virtual  arc 

=  time  to  go  to  intercept 

=  initial  interceptor  flight  path  angle 

=  initial  rate  of  change  of  interceptor  flight  path  angle 
=  final  interceptor  flight  path  angle 
=  optimal  path  time  history 
=  initial  interceptor  velocity 
=  final  interceptor  velocity 
=  initial  interceptor  acceleration 

=  the  optimal  path  time  history  in  Cartesian  coordinates 
=  initial  interceptor  position 
=  initial  interceptor  velocity 
=  initial  interceptor  acceleration 
=  final  interceptor  velocity 
=  current  target  velocity 
=  current  target  position 


[path] =SMTra j ectory (free,  const) ; 


global  calccost  costs  q  qq  tgo  trys 

%  Initialize  Variables 

qq=qq+l; 

dist=0 ; 

Re=6 .378137e6; 


%%  Identify  Variables 

time=path ( : , 1 ) ; 

X=path ( : , 2  :  4 )  ; 

V=path ( : , 5 ) ; 
th=path ( : , 6) ; 
psi=path ( : , 7 ) ; 
Vdot=path  ( : , 8 ) ; 
thdot=path ( : , 9) ; 
psidot=path ( : , 10) ; 
nx=path  (  : , 1 1 )  ; 
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ny=path ( : , 12 ) ; 
nz=path ( : , 13) ; 

N=length (path (  :  ,  1 )  )  ; 

tau_f=f ree ( 1 ) ; 
tgo=free  (2 ) ; 
thf=f ree  ( 3 )  ; 
psif=f ree  ( 4 ) ; 

xO=const (1:3) ; 
xdO=const (4:6) ; 
xddO=const  (7:9)  ; 
t=const  (10)  ; 
xt=const (11:13)  ; 
xdt=const (14:16) ; 
init=const (17:19) ; 

V_f=path (N, 5) ; 

xdf= [V_f *cos (thf ) *cos  (psif )  ; 

V_f*cos (thf) *sin (psif) ; 

V_f *sin (thf) ] ; 
tgo=path (N, 1 ) ; 

for  i=2 : 1 :  N 

dist=dist+abs (norm( [X(i, 1) -X ( i-1 , 1) ; X (i, 2 ) -X ( i-1 , 2) ;X(i,  3) -X  (i- 
1,3);])) ; 
end 

nmax=4  0+ (40-10)/ (0-50000)* (path ( : ,  4) -Re)  ; 

%%  Calculate  Cost  of  the  chosen  trajectory 
J= [  tau_f; 
tgo; 

100*abs (dot (xdf , xdt) / norm (xdf ) / norm (xdt) ) ] ; 

Py=sum (max ( 0 , abs (ny) -nmax) . A2 ) ; 

Pz=sum (max ( 0 , abs (nz ) -nmax)  . A2  )  ; 

cost=0 . 33*ones (1,3) * J+norm ( [ Py; Pz ] ) ; 
costs (qq, 1:6)  =  [cost; J; Py;  Pz;  ]  ; 
calccost=cost; 


return 


3.  SMTrajectory.m 

function  [path] =SMTra j ectory (free, const) 

%  Written  by  LT  John  A.  Lukacs  IV,  Naval  Postgraduate  School,  June  2006 

%  This  function  proposes  a  trajectory  based  on  the  input  parameters 
%  "free"  and  "const".  This  is  a  sub-function  of  the  SMGuidance.m 
%  function's  fminsearch.  This  function  creates  a  7th  order  set  of 
%  equations  and  evaluates  that  equation  at  the  boundary  conditions 
%  supplied  by  the  inputs.  It  then  calculates  the  time  history  of  all 
%  the  flight  vehicle  variables,  including  controls  and  reactions. 
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necessary  to  develop  that  flight  path.  A  plot  command  set  at  the  end 
of  this  function  will  plot  a  chart  of  the  iterations  at  the  end  of 
run  if  desired. 


Variable  List 

A  =  matrix  of  reference  equations 

Ax  =  boundary  condition  reference  equation  (1  for  each  axis) 

Axp  =  1st  derivative  of  boundary  conditions  reference  equation 

(1  for  each  axis) 

Axpp  =  2nd  derivative  of  boundary  conditions  reference  equation 

(1  for  each  axis) 

Axppp  =  3rd  derivative  of  boundary  conditions  reference  equation 

(1  for  each  axis) 

BND  =  boundary  condition  vector  (1  for  each  axis) 

BR  diff  =  iterative  variable  (used  for  plotting) 

BR_end  =  iterative  variable  (used  for  plotting) 

const  =  variables  that  fminsearch  cannot  modify,  including  system 

constraints,  specifically  [xO ; xdO ; xddO ; time; xt ; xdt ; init ] 
Cx  =  inversion  of  Ax  (1  for  each  axis) 

Cxp  =  inversion  of  Axp  (1  for  each  axis) 

Cxpp  =  inversion  of  Axpp  (1  for  each  axis) 

Cxppp  =  inversion  of  Axppp  (1  for  each  axis) 

dtau  =  tau  step  value 

dtime  =  time  step  value 

free  =  variables  that  fminsearch  can  modify,  specifically 

[tau; tgo; thf ;psif ] 
g  =  gravitational  force 

i  =  index  for  coordinate  axis 

init  =  vector  of  initial  estimates 

L  =  lambda,  virtual  speed 

Lf  =  final  lambda,  defined  as  Vf 

Li  =  initial  lambda,  defined  as  VO 

Lpf  =  1st  derivative  of  final  lambda 

Lpi  =  1st  derivative  of  initial  lambda 

nmax  =  maximum  acceleration  capability  of  the  interceptor, 

altitude  dependent 

nX  =  norm  of  reference  trajectory 

nx  =  axial  acceleration  command,  body  frame  x 

nXp  =  norm  of  1st  derivative  of  reference  trajectory 

nXpp  =  norm  of  2nd  derivative  of  reference  trajectory 

nXppp  =  norm  of  3rd  derivative  of  reference  trajectory 

ny  =  axial  acceleration  command,  body  frame  y 

nz  =  axial  acceleration  command,  body  frame  z 

path  =  returned  time  history  of  the  optimal  path,  specifically 

[time '  X (1 : 3 , : ) '  V'  th '  psi '  Vdot '  thdot '  psidot '  nx ' . . . 
ny '  nz ' ] 

psi  =  interceptor  heading  angle 

psidotf  =  final  rate  of  change  of  interceptor  heading  angle 

psif  =  final  interceptor  heading  angle 

psip  =  1st  derivative  of  heading  angle 

qq  =  counting  variable 

t  =  current  time 

tau  =  virtual  arc  variable 

tau  f  =  value  of  the  virtual  arc 

tgo  =  time  to  go  to  intercept 

th  =  initial  interceptor  flight  path  angle 

thdotf  =  final  rate  of  change  of  interceptor  flight  path  angle 
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%  thf 
%  thp 
%  time 
%  trys 

O, 

o 

%  V 

%  Vdotf 
%  Vdoti 
%  Vf 
%  Vi 
%  Vp 
%  X 
%  xO 
%  xdO 
%  xddO 
%  xddf 
%  xdf 
%  xdt 
%  xf 
%  xF 
%  xl 
%  Xp 
%  xpO 
%  xpf 
%  xpF 
%  xpl 
%  Xpp 
%  xppO 
%  xppf 
%  xppF 
%  xpp  I 
%  Xppp 
%  xpppO 
%  xpppf 
%  xpppF 
%  xppp I 
%  xt 


=  final  interceptor  flight  path  angle 
=  1st  derivative  of  flight  path  angle 
=  optimal  path  time  history 

=  collection  of  norms  [X  nX  Xp  nXp  Xpp  nXppp]  (used  for 
plotting) 

=  velocity 

=  final  acceleration 

=  initial  acceleration 

=  final  velocity 

=  initial  velocity 

=  1st  derivative  of  velocity 

=  reference  trajectory  (1  for  each  axis) 

=  initial  interceptor  position 
=  initial  interceptor  velocity 
=  initial  interceptor  acceleration 
=  final  interceptor  position 
=  final  interceptor  velocity 
=  current  target  velocity 
=  estimate  of  intercept  position 
=  final  position  [xl;x2]  boundary  condition 
=  initial  position  [xl;x2]  boundary  condition 
=  1st  derivative  of  reference  trajectory  (1  for  each  axis) 
=  1st  derivative  of  initial  boundary  conditions 
=  1st  derivative  of  final  boundary  conditions 
=  final  1st  derivative  of  position  boundary  condition 
=  initial  1st  derivative  of  position  boundary  condition 
=  2nd  derivative  of  reference  trajectory  (1  for  each  axis) 
=  2nd  derivative  of  initial  boundary  conditions 
=  2nd  derivative  of  final  boundary  conditions 
=  final  2nd  derivative  of  position  boundary  condition 
=  initial  2nd  derivative  of  position  boundary  condition 
=  3rd  derivative  of  reference  trajectory  (1  for  each  axis) 
=  3rd  derivative  of  initial  boundary  conditions 
=  3rd  derivative  of  final  boundary  conditions 
=  final  3rd  derivative  of  position  boundary  condition 
=  initial  3rd  derivative  of  position  boundary  condition 
=  current  target  position 


global  update  trys  q  qq  Pos_BR  Pos_SM  calccost 
Re= 6 .378137e6; 


%%  Define  Terms 
tau_f=free  (1)  ; 
tgo=free  (2 ) ; 
thf =f ree  (3 ) ; 
psif =f ree  ( 4 ) ; 

xO=const (1:3) ; 
xdO=const (4:6) ; 
Vi=norm(xdO) ; 
Li=Vi ; 

xddO=const (7:9) ; 
Vdoti=norm (xddO ) ; 
Lpi=Vdoti/Vi; 
t=const (10) ; 
xt=const (11:13) ; 
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xdt=const (14:16)  ; 
init=const (17:19) ; 


Vf=3500-10*tgo; 

Lf=Vf ; 

xf=xt+xdt . * (2 . 5*tgo)  ; 
xdf= [Vf *cos (thf ) *cos (psif )  ; 

Vf*cos (thf) *sin (psif) ; 

Vf *sin (thf) ] ; 

Vdotf =-5 .9578; 
thdotf =0 ; 
psidotf=0 ; 

xddf= [Vdotf*cos (thf) *cos (psif) -Vf*cos (thf) *sin (psif) *psidotf 
Vf*sin (thf) *cos (psif) * thdotf  ; 

Vdotf*cos (thf) *sin (psif) +Vf*cos (thf) *cos (psif) *psidotf 
Vf *sin (thf) *sin (psif) * thdotf  ; 

Vdotf*sin (thf) +Vf*cos (thf) *thdotf ]  ; 

Lpf=Vdotf /Vf ; 

%%  Calculate  Coefficients 
xp0=xd0/Li; 

xpp0= (xddO-xdO*Lpi ) /LiA2; 
xppp0= [0; 0; 0]  ; 
xpf=xdf /Lf ; 

xppf= (xddf-xdf *Lpf ) /Lf  A2 ; 
xpppf = [ 0 ; 0 ; 0 ]  ; 

xp0=xd0 ; 
xpp0=xdd0 ; 
xpf=xdf ; 
xppf=xddf ; 

syms  tau  F  xl  xpl  xppl  xpppl  xF  xpF  xppF  xpppF  real 


1  0 

0 

0 

0 

0.  .  . 

0 

0; 

0  1 

0 

0 

0 

0.  .  . 

0 

0; 

0  0 

1 

0 

0 

0.  .  . 

0 

0; 

0  0 

0 

1 

0 

0.  .  . 

0 

0; 

1  tau  F 

tau 

.  FA 

2/2  tau 

FA3/6 

tau 

_FA4/24 

tau 

FA 

5/60 

tau  FA6/120 

tau 

FA7/210; 

0  1 

tau 

.  F 

tau 

FA2/2 

tau 

_FA3/6 

tau 

FA 

4/12 

tau  FA5/20 

tau 

_F  A  6  /  3  0  ; 

0  0 

1 

tau 

F 

tau 

_FA2/2 

tau 

FA 

3/3. 

tau  FA4/4 

tau 

F  A  5  /  5  ; 

0  0 

0 

1 

tau 

F 

tau 

FA 

2  .  .  . 

tau  FA3 

tau 

FA4]  ; 

b=[xl  xpl  xppl  xpppl  xF  xpF  xppF  xpppF] 
a=A\b; 

a=collect (a, tau_F) ; 

N=length (a) ; 

%%  Define  Boundary  Conditions 

%  { ' xl '  ,  ' xpl ' ,  ' xppl ' ,  ' xF ' ,  ' xpF ' ,  ' xppF ' ,  ' tau  F ' } 
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BND { 1 } = { xO ( 1 ) , xpO ( 1 ) , xppO ( 1 ) , xpppO ( 1 ) , xf ( 1 ) , xpf ( 1 ) , xppf ( 1 ) , . . . 
xpppf (1) , tau_f } ;  %xl 

BND {2 }  =  {xO (2) , xpO (2) , xppO (2) , xpppO (2) , xf (2) , xpf (2)  ,  xppf (2)  ,  .  .  . 
xpppf (2) , tau_f } ;  %x2 

BND { 3 } = { xO ( 3 ) , xpO ( 3 ) , xppO ( 3 ) , xpppO ( 3 ) , xf ( 3 ) , xpf ( 3 ) , xppf ( 3 ) , . . . 

xpppf ( 3 ) , tau_f } ;  %x3 
dtau=tau_f/99; 

tau= [ 0 : dtau : tau_f ] ;  %  100  data  points 
clear  A 

%%  Calculate  trajectories  (2+1  4th  order  case) 
for  i=l : 3 

A{  i }=subs (a,  { ' xl ' ,  ' xpl ' ,  ' xppl ' ,  ' xpppl ' ,  ' xF ' ,  ' xpF ' ,  ' xppF ' ,  .  .  . 
' xpppF ' , ' tau_F ' } , BND { i } ) ; 


Ax { i } =diag ( [  1 , 

*A{ i } ; 

1,  1/2,  1/6, 

1/24, 

1/60, 

1/120, 

1/210 

]  ) 

Axp { i } =diag ( [  0 , 
*A{ i } ; 

1,  1,  1/2, 

1/6, 

1/12, 

1/20, 

1/30 

]  ) 

Axpp { i } =diag ( [  0, 
*A{ i } ; 

\ — 1 

\ — 1 

o 

1/2, 

1/3, 

1/4, 

1/5 

]  ) 

Axppp { i } =diag ( [  0, 
*A{ i } ; 

1 — 1 

o 

o 

1, 

1, 

1, 

1 

]  ) 

Cx (i, : ) =Ax { i } ( [N:-l:l] ) ; 

Cxp ( i , : ) =Axp { i } ( [N : - 1 : 2 ] ) ; 

Cxpp ( i ,  : ) =Axpp { i }  ( [N : - 1 : 3 ] ) ; 

Cxppp ( i , : ) =Axppp { i } ( [N 1:4] ) ; 

X  ( i ,  : ) =polyval (Cx ( i ,  : ) , tau) ; 

Xp (i,  : ) =polyval (Cxp (i,  : ) , tau) ; 

Xpp (i, : ) =polyval (Cxpp (i, : ) , tau) ; 
Xppp (i, : ) =polyval (Cxppp (i, : ) , tau) ; 

end 


%%  Compute  the  States 

g=3 . 98 60044 18el4/norm (X (1 : 3 , 1 ) + [ 0 ; 0 ; Re] ) A2; 

V ( 1 ) =Vi ; 

Vp  ( 1 ) =Vdoti ; 

th ( 1 ) =atan2 (Xp ( 3 , 1 ) , norm (Xp ( 1 : 2 , 1 ) ) ) ; 
psi ( 1 ) =atan2 (Xp  (2 , 1)  ,  Xp  (1, 1)  )  ; 

L ( 1 ) =V ( 1 ) ; 

time(l)=t;  dtime (1) =0 . 1; 
tau ( 1 ) =0 ; 

for  j=2 : length (tau) ; 

g=3 . 986004418el4/norm (X (1 : 3 , j -1 )  +  [ 0 ; 0 ;  Re] ) A2 ; 
if  norm (X (:, j -1 )) <86000 

[ro, press, temp] =STatmos (norm (X ( : , j -1) ) ) ; 

else 

[ro, press , temp] =STatmos ( 86000 )  ; 

end 

MV=V ( j -1 ) / sqrt ( 1 . 4 02 *2  87 * temp) ; 

[m_i, dia] =SMParams3 (time ( j -1)  )  ; 

[CD] =ZLDragC (MV, time ( j -1 ) ) ; 
if  time ( j -1 )  <  6 
Thrust=2 06000 ; 

CD=CD (1) ; 
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elseif  time(j-l)  <  26 
Thrust=95300; 

CD=CD ( 1 ) ; 

else 

Thrust=0 ; 

CD=CD (2) ; 

end 

Sref=pi*diaA2/ 4; 

Drag=ro*V (j-1) A2*CD*Sref /2 ; 
nx ( j -1 ) = (Thrust -Drag) /m_i/g; 

th ( j -1 ) =atan2 (Xp ( 3 , j -1 ) , norm (Xp ( 1 : 2 , j -1 ) ) ) ; 
psi  ( j  -1)  =atan2  (Xp  (2,  j  -1)  ,  Xp  (1,  j  -1)  )  ; 

Vp ( j-1) =g* (nx ( j-1) -sin (th ( j-1) ) ) ; 

thp (j-1) —cos (th ( j-1) A2) * (Xpp ( 3 , j - 1 ) * ( Xp ( 1 , j - 1 ) A2+Xp (2, j-1) A2) 
Xp (3, j -1) * (Xpp (1, j-1) +Xpp ( 2 , j - 1 ) ) ) / ( Xp ( 1 , j - 1 ) A  2  + .  .  . 
Xp ( 2 , j - 1 ) A2) A1.5; 

psip ( j-1) —cos (psi (j-1) A2) * (Xp ( 1 , j -1 ) *Xpp (2, j-1) -Xpp (1, j-1) * . . 

Xp(2,  j-1)  )  /  (Xp(l,  j-1)  A2+Xp(2,  j-1)  A2 )  ; 
ny ( j -1) =V ( j -1) /g*cos (th ( j -1) ) *psip (j-1) ; 
nz (j-1) =V(j-l) / g*thp (j-1) +cos (th ( j-1) ) ; 

v( j ) =V( j-1) +Vp (j-1) *dtime (j-1) ; 

tau ( j ) =tau ( j -1 ) +dtau; 

ddist ( j ) =sqrt ( (X(l,j)-X(l,j-1) ) A2+(X(2,j)-X(2,j-l) )A2+.  .  . 

(X  ( 3 ,  j  )  -X  (3 ,  j  -1 )  )  A2)  ; 
dtime  ( j ) =2*ddist ( j ) / (V ( j ) +V ( j-1) ) ; 

L ( j ) =dtau/ dtime ( j -1 ) ; 

time ( j ) =time ( j -1 ) +dtime ( j -1 ) ; 

end 

N=length (X ( 1 , : ) ) ; 
for  i=l : 1 : N 

nX ( i ) =norm (X ( 1 : 3 , i ) ) ; 
nXp ( i ) =norm (Xp ( 1 : 3 , i ) ) ; 
nXpp ( i ) =norm (Xpp ( 1 : 3 , i ) ) ; 

end 

dtime (N) =dtime (N-l )  ; 
th (N) =th (N-l) ; 
psi  (N) =psi (N-l ) ; 

Vp (N) =Vp (N-l) ; 
thp (N) =thp (N-l ) ; 
psip (N) =psip (N-l ) ; 
nx (N) =nx (N-l ) ; 
ny (N) =ny (N-l ) ; 
nz (N) =nz (N-l ) ; 

trys=[X'  nX '  Xp '  nXp '  Xpp'  nXpp ' ] ; 

path= [time '  X (1 : 3 , : ) '  V'  th'  psi'  Vp '  thp '  psip '  nx '  ny '  nz '  tau ' 
ddist'  dtime'  L ' ] ; 

%%  Plot  every  iteration 

nmax=4  0+ (40-10) / (0-50000) * (path ( : ,  4) -Re)  ; 
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BR  end=time (N) *2+1 ; 

BR  diff=BR  end; 
while  BR  diff>l 

BR  diff=BR  diff-1; 

end 

BR  end=BR  end-BR  diff; 

figure (100) 
f igurepalette ( ' hide ' ) 
subplot (3, 6,  [1  2  7  8]) 

plot3 (X ( 1 , :) ,X(2, :) ,X(3, : ) -Re, ' Linewidth ' , 3 ) ; grid  on 
hold  on; 

plot3 ( Pos_BR ( 1 : 300,1), Pos_BR(l : 300,2), Pos_BR(l : 300,3) -Re,  '  :  k'  ,  .  .  . 

' LineWidth ' , 2 ) 

plot3 ( Pos_BR ( 14  0+BR_end, 1) , Pos_BR ( 14 0+BR_end, 2 ) ,  .  .  . 

Pos  BR(140+BR  end,3)-Re, 'ok', ' LineWidth ', 3 ) 
plot3(Pos  BR ( 14 0 , 1 ) , Pos  BR ( 14 0 , 2 ) , Pos  BR ( 14 0 , 3 ) -Re, ' *k ' , ' LineWidth ' , 2 ) 
plot 3 (xf (1) ,xf (2) ,xf (3) -Re, ' *r ' , ' LineWidth ' , 2 ) 

plot3 (Pos_SM(l : 19, 1) , Pos_SM ( 1 : 1 9 , 2 ) , Pos_SM(l : 19,3) -Re, ' — b' , . . . 

' LineWidth ' , 2 ) 
view (-102, 8) 
hold  off; 

axis([0  1 . Ie5  -le4  1 . Ie5  0  7e4]) 
title (' 3D  Flight  Path') 
subplot (3, 6,  [13  14]) 

plot (path ( : , 1 ) , path ( : , 12 ) , ' --b ' , ' Linewidth ' , 3 ) 
hold  on; grid  on 

plot (path ( : , 1 ) , path ( : , 1 3 ) , ' -k ' , ' Linewidth ' , 3 ) 

plot (path ( : , 1 ) , nmax ( : ) , ' --r ' , ' Linewidth ' , 3 ) 

plot (path ( : , 1 ) , -nmax ( : ) , ' --r ' , ' Linewidth ' , 3 ) 

axis ([10  time (N)  -45  45]); 

title (' Control  Effort') 

hold  off 

subplot (3,6,3) 

plot (time, X ( 1 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  le4  l.le5]) 
title ( 'X (1) ' ) 
subplot (3,6,9) 

plot (time, X (2 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -le4  l.le5]) 
title ( 'X (2) ' ) 
subplot (3, 6, 15) 

plot (time, X ( 3 , : ) -Re, ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  0  7e4]) 
title ( 'X  (3)  '  ) 
subplot (3,6,4) 

plot (time, Xp ( 1 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title ( ' X_p ( 1 ) ' ) 
subplot (3, 6,  10) 

plot (time, Xp (2 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title ( ' X_p (2 ) ' ) 
subplot (3, 6, 16) 

plot (time, Xp ( 3 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title ( ' X_p ( 3 ) ' ) 
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subplot (3,6,5) 

plot (time, Xpp ( 1 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title  (  '  X_p_p  ( 1 )  '  ) 
subplot (3,6,11) 

plot (time, Xpp (2 , : ) , ' Linewidth ' , 3 ) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title ( ' X_p_p (2 ) ' ) 
subplot (3,6,17) 

plot (time, Xpp ( 3 , : ) , ' Linewidth ' , 3) ; grid  on 
axis ([10  time (N)  -45  45]);axis  'auto  y' 
title ( ' X_p_p ( 3 ) ' ) 
subplot (3,6,6) 
hold  on 

plot (qq, time (N) , ' *k ' , ' Linewidth ' , 1 ) ; grid  on 
hold  off 

axis ( [0  200  0  60]); 
title ('Time  to  go') 
if  qq>l 

subplot (3, 6,  [12  18]) 
hold  on 

plot(qq,calccost, ' *k ' , ' Linewidth ' , 1) ; grid  on 
axis([0  200  0  100]);axis  'auto  y' 
hold  off 

title (' Functional  Cost') 
end 


return 


138 


LIST  OF  REFERENCES 


1.  Alway,  Peter,  “Tartar  Missile  Dimensions”, 
http://yellowiacketsystems.com/alway/images/tartar.gif.  Accessed  1/10/2006 

2.  Bardanis,  Florios,  “Kill  Vehicle  Effectiveness  for  Boost  Phase  Interception  of 
Ballistic  Missiles”,  NPS  Masters  Thesis,  2004 

3.  Lennox,  Duncan,  “Ballistic  Missile  Defense,”  Jane’s  Strategic  Weapon  Systems  40, 
p.  4,  November  27,  2003 

4.  Federation  of  American  Scientists,  “Taep’o-dong  2”, 

http ://www. fas . org / nuke/ guide/ dprk/missile/td-2 . htm.  Accessed  08/27/2005 

5.  Google  Earth™  distance  calculator,  Accessed  06/10,2006 

6.  Jane’s  Strategic  Advisory  Services,  “RIM-66/-67/-156  Standard  SM-1/-2  and  RIM- 
161  SM-3”, 

http://www4.ianes.com/K2/doc.isp?K2DocKev=7contentl/ianesdata/binder/isws/isws 

0208.htm,  Accessed  10/26/2005 

7.  Hemsch,  M.J.  (editor),  Tactical  Missile  Aerodynamics:  General  Topics. 
“Aerodynamic  Considerations  for  Autopilot  Design,”  by  L.L.Cronvich,  Progress  in 
Astronautics  and  Aeronautics,  Vol.141,  1992 

8.  Hutchins,  Robert,  ME4703  “Missile  Flight  Analysis”  Course  Notes,  Spring  2005 

9.  Missile  Defense  Agency,  “Ballistic  Missile  Defense  System  Overview”, 
http://www.mda.mil/mdalink/pdf/bmdsbook.pdf.  Accessed  05/15/2006 

10.  NASA,  “Solid  Propellant  Grain  Design  and  Internal  Ballistics”,  Document  SP-8076, 
March  1972 

11.  Raytheon,  “Missile  Trajectory  Phases”, 
http://www.ravtheonmissiledefense.com/phases/index.html.  Accessed  05/15/2006 

12.  Raytheon,  “Standard  Missile  6:  Extended  Range  Active  Missile”, 
http://www.ravtheon.com/products/stellent/groups/public/documents/content/cms04 

014817.pdf.  Accessed  01/10/2006 

13.  San  Jose,  Antonio,  “Theater  Ballistic  Missile  Defense  -  Multisensor  Fusion, 
Targeting,  and  Tracking  Techniques”,  NPS  Masters  Thesis,  1998 

14.  Stevens,  Brian,  and  Lewis,  Frank,  Aircraft  Control  and  Simulation  Second  Edition, 
John  Wiley  and  Sons,  2003 


139 


15.  Springer  Online  Reference  Works,  “Calculus  of  Variations”, 
http://eom.springer.de/v/v096 190.htm,  Accessed  06/10/2006 

16.  Venik’s  Aviation,  “Russian  Space  Engines”, 
http://www.aeronautics.ru/archive/reference/Russian  Space  Engines/Russian  Space 

Engines  22.htm,  Accessed  06/10/2006 

17.  Wikipedia,  “Calculus  of  Variations”,  “Galerkin  method”,  “Rayleigh-Ritz  method”, 
http://en.wikipedia.org/wiki/Calculus  of  variations. 

http://en.wikipedia.org/wiki/Galerkin  method,  http ://en. wikipedia. org/wiki/Rayleigh- 
Ritz  method.  All  accessed  06/10/2006 

18.  Yakimenko,  O.A.,  AE4903  “Direct  Method  of  Calculus  of  Variations  as  the  Means 
for  Rapid  Prototyping  of  Optimal  Trajectories”  Course  Notes,  Winter  2006 

19.  Yakimenko,  O.A.,  “Direct  Methods  for  Rapid  Prototyping  of  Near-Optimal  Aircraft 
Trajectories”,  Journal  of  Guidance,  Control,  and  Dynamics,  Vol  23,  No.  5,  Sep-Oct 
2000,  pp  865-875 

20.  Zarchan,  Paul,  “Ballistic  Missile  Defense  Guidance  and  Control  Issues”,  Science  & 
Global  Security,  Volume  8,  1998,  pp.  99-124 

21.  Zarchan,  Paul,  Tactical  and  Strategic  Missile  Guidance  Fourth  Edition,  Vol.  199, 
American  Institute  of  Aeronautics  and  Astronautics,  Reston,  VA,  2002 

22.  Zipfel,  Peter,  Modeling  and  Simulation  of  Aerospace  Vehicle  Dynamics,  American 
Institute  of  Aeronautics  and  Astronautics,  Reston,  VA,  2000 


140 


INITIAL  DISTRIBUTION  LIST 


1.  Defense  Technical  Infonnation  Center 
Ft.  Belvoir,  Virginia 

2.  Dudley  Knox  Library 
Naval  Postgraduate  School 
Monterey,  California 

3.  Temasek  Defence  Systems  Institute 
National  University  of  Singapore 
Singapore,  Singapore 

4.  Thomas  Hoivik,  Associate  Chair  of  Interdisciplinary  Activities 
Naval  Postgraduate  School 

Monterey,  California 


141 


